/*
 * OBBTree.h
 *
 * Created 6/3/2009 By Johnny Huynh
 *
 * Version 00.00.04 7/4/2009
 *
 * Copyright Information:
 * All content copyright © 2009 Johnny Huynh. All rights reserved.
 */

 /**
  * If GL_Object.h is not being used in conjunction with this OBBTree.h,
  * GL_OBJECT_COLLISION_DETECTION_TYPE should probably be defined as 0.
  *
  * #define GL_OBJECT_COLLISION_DETECTION_TYPE = 0
  */
 
 #ifndef OBBTREE_H
 #define OBBTREE_H
 
 #include "BinaryTree_Interface.h"
 
 template <typename TYPENAME> class OBBTree;

 typedef OBBTree<GLfloat> OBBTreef;
 typedef OBBTree<GLdouble> OBBTreed;

 #include <vector>
 #include "GL_Triangle.h"
 #include "OBB.h"
 #include "introsort.h"
 #include "ConvexHull.h"
 #include "CollisionInfo.h"
 
 template <typename TYPENAME>
 class OBBTree : public BinaryTree_Interface< OBBTree<TYPENAME> >
 {
 // Private Classes
 private:
    template <typename TYPENAME> class Triangle_Projection_Info;
    template <typename TYPENAME> class Split_OBB_Info;
 
 // Data Members
 protected:
	OBB<TYPENAME> obb; // oriented-bounding box for this set of triangles
	//std::vector< const GL_Triangle<TYPENAME>* > raw_triangles; // the positions of the raw triangles needs to be relative to the 
	                                                     // object at position (0, 0, 0) and orientation( 1.0f, 0.0f, 0.0f,
	                                                     //                                               0.0f, 1.0f, 0.0f,
	                                                     //                                               0.0f, 0.0f, 1.0f )
	const GL_Object<TYPENAME>* object; // pointer to the position and orientation of the object
	std::vector< const GL_Triangle<TYPENAME>* > triangle_Ptrs; // pointers to the current state (i.e. up-to-date) triangles

 // Local Functions
 public:
    /*OBBTree( const GL_Object<TYPENAME>* const object, int maxDepth = -1,
             const std::vector< const GL_Triangle<TYPENAME>* >& raw_triangle_Addresses = std::vector< const GL_Triangle<TYPENAME>* >(),
             const std::vector< const GL_Triangle<TYPENAME>* >& triangle_Addresses = std::vector< const GL_Triangle<TYPENAME>* >() );*/
             //const Vector3<TYPENAME>& position = Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ),
             //const OrientationMatrix3<TYPENAME>& orientation = OrientationMatrix3<TYPENAME>( -1.0f, 0.0f, 0.0f,
                                                                                              //0.0f, 1.0f, 0.0f,
    
    OBBTree( const GL_Object<TYPENAME>* const object, int maxDepth = -1,
             const GL_Triangle<TYPENAME>* const * const raw_triangle_Ptrs_Ptr = NULL, 
             const GL_Triangle<TYPENAME>* const * const triangle_Ptrs_Ptr = NULL, size_t size = 0x0 );
    OBBTree( const GL_Object<TYPENAME>* const object, int maxDepth,
             const GL_Triangle<TYPENAME>* const raw_triangles_Ptr, 
             const GL_Triangle<TYPENAME>* const triangles_Ptr, size_t size );
    OBBTree( const OBBTree<TYPENAME>& obbtree, const GL_Object<TYPENAME>* const object, int maxDepth = -1 );
    ~OBBTree();
    inline OBBTree<TYPENAME>& operator=( const OBBTree<TYPENAME>& obbtree );
 
 // Private Functions
 private:
    inline void copyTriangles( const OBBTree<TYPENAME>& obbtree );
    //inline Matrix3<TYPENAME> covariance( std::vector< const GL_Triangle<TYPENAME> * > triangles ) const;
    inline Matrix3<TYPENAME> covariance( const GL_Triangle<TYPENAME> * const triangles_Ptr, const size_t size ) const;
    //inline OBB<TYPENAME> computeOBB( const std::vector< const GL_Triangle<TYPENAME>* >& raw_triangles ) const;
    inline OBB<TYPENAME> computeOBB( const GL_Triangle<TYPENAME>* const raw_triangles_Ptr, const size_t size ) const;
    inline OBB<TYPENAME> computeOBB( const GL_Triangle<TYPENAME>* const * const raw_triangle_Ptrs_Ptr, 
                                                                                    const size_t size ) const;
    inline std::vector< Vector3<TYPENAME> > getSortedRawAxes();
    inline void splitNextLevel( const GL_Triangle<TYPENAME>* const * const raw_triangle_Ptrs_Ptr, 
                                                                                      const size_t size, const int maxDepth );
    inline Split_OBB_Info<TYPENAME> getSplitOBBInfo( const GL_Triangle<TYPENAME>* const * const raw_triangle_Ptrs_Ptr, 
       const GL_Triangle<TYPENAME>* const * const triangle_Ptrs_Ptr, const size_t size, const Vector3<TYPENAME>& axis ) const;
 
 // Friend Functions
 public:
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 0
    template <typename TYPENAME> friend inline bool collide( const OBBTree<TYPENAME>& obbtreeA, 
                                                             const OBBTree<TYPENAME>& obbtreeB,
                                                             int maxDepth = -1 );
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 0
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
    template <typename TYPENAME> friend inline bool collide( OBBTree<TYPENAME>& obbtreeA, 
                                                             OBBTree<TYPENAME>& obbtreeB,
                                                             int maxDepth );
    template <typename TYPENAME> friend inline bool collide( OBBTree<TYPENAME>& obbtreeA, OBBTree<TYPENAME>& obbtreeB );
    template <typename TYPENAME> friend inline bool collide( OBBTree<TYPENAME>& obbtreeA, OBBTree<TYPENAME>& obbtreeB, 
                                                             size_t maxDepthA, size_t maxDepthB );
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 #if IMPLEMENT_INTERPOLATION == 1
    template <typename TYPENAME> friend inline CollisionInfo<TYPENAME> computeCollisionInfo( OBBTree<TYPENAME>& obbtreeA, 
                                 OBBTree<TYPENAME>& obbtreeB, int maxDepth, 
                                 Vector3<TYPENAME> displacementA, Vector3<TYPENAME> displacementB );
    template <typename TYPENAME> friend inline CollisionInfo<TYPENAME> computeCollisionInfo( OBBTree<TYPENAME>& obbtreeA, 
            OBBTree<TYPENAME>& obbtreeB, Vector3<TYPENAME> displacementA, Vector3<TYPENAME> displacementB );
    template <typename TYPENAME> friend inline CollisionInfo<TYPENAME> computeCollisionInfo( OBBTree<TYPENAME>& obbtreeA, 
                                        OBBTree<TYPENAME>& obbtreeB, size_t maxDepthA, size_t maxDepthB, 
                                        Vector3<TYPENAME> displacementA, Vector3<TYPENAME> displacementB );
    template <typename TYPENAME> friend inline TYPENAME collisionExtent( OBBTree<TYPENAME>& obbtreeA, 
                                 OBBTree<TYPENAME>& obbtreeB, int maxDepth, 
                                 const Vector3<TYPENAME>& displacementA, const Vector3<TYPENAME>& displacementB );
    template <typename TYPENAME> friend inline TYPENAME collisionExtent( OBBTree<TYPENAME>& obbtreeA, 
            OBBTree<TYPENAME>& obbtreeB, const Vector3<TYPENAME>& displacementA, const Vector3<TYPENAME>& displacementB );
    template <typename TYPENAME> friend inline TYPENAME collisionExtent( OBBTree<TYPENAME>& obbtreeA, 
                                        OBBTree<TYPENAME>& obbtreeB, size_t maxDepthA, size_t maxDepthB, 
                                        const Vector3<TYPENAME>& displacementA, const Vector3<TYPENAME>& displacementB );
    template <typename TYPENAME>
 friend inline CollisionInfo<TYPENAME> collideE( OBBTree<TYPENAME>& obbtreeA, OBBTree<TYPENAME>& obbtreeB, int maxDepth );
 template <typename TYPENAME>
 friend inline CollisionInfo<TYPENAME> collideE( OBBTree<TYPENAME>& obbtreeA, OBBTree<TYPENAME>& obbtreeB );
    template <typename TYPENAME>
 friend inline CollisionInfo<TYPENAME> collideE( OBBTree<TYPENAME>& obbtreeA, OBBTree<TYPENAME>& obbtreeB, size_t maxDepthA, size_t maxDepthB );
    
 #endif // IMPLEMENT_INTERPOLATION == 1
    template <typename TYPENAME> friend inline void maintain( OBBTree<TYPENAME>& obbtree );
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
    template <typename TYPENAME> friend inline void markForMaintenance( OBBTree<TYPENAME>& obbtree );
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
    template <typename TYPENAME> friend inline void print( const OBBTree<TYPENAME>& obbtree, GLint depth = 0x0 );
    template <typename TYPENAME> friend inline void render( const OBBTree<TYPENAME>& obbtree, GLint depth = 0x0 );
    
 // Private Classes
 private:
     template <typename TYPENAME>
     class Triangle_Projection_Info
     {
     // Data Members
     public:
        const GL_Triangle<TYPENAME>* raw_triangle_Ptr;
        const GL_Triangle<TYPENAME>* triangle_Ptr;
        TYPENAME minProjection;
        TYPENAME maxProjection;
     
     // Local Functions
        Triangle_Projection_Info( const GL_Triangle<TYPENAME>* raw_triangle_Ptr, const GL_Triangle<TYPENAME>* triangle_Ptr, 
                                  const TYPENAME& minProjection, const TYPENAME& maxProjection ) 
                                  : raw_triangle_Ptr( raw_triangle_Ptr ), triangle_Ptr( triangle_Ptr ), 
                                    minProjection( minProjection ),
                                    maxProjection( maxProjection ){}
        Triangle_Projection_Info() : raw_triangle_Ptr( NULL ), triangle_Ptr( NULL ), 
                                     minProjection( 0.0f ), maxProjection( 0.0f ){}
        ~Triangle_Projection_Info(){}
        inline Triangle_Projection_Info<TYPENAME>& operator=( const Triangle_Projection_Info<TYPENAME>& tpi )
                                                             { 
                                                                raw_triangle_Ptr = tpi.raw_triangle_Ptr;
                                                                triangle_Ptr = tpi.triangle_Ptr;
                                                                minProjection = tpi.minProjection;
                                                                maxProjection = tpi.maxProjection;
                                                                
                                                                return *this;
                                                             }
     
     // Friend Functions
        template <typename TYPENAME> friend inline bool operator<( const Triangle_Projection_Info<TYPENAME>& a, 
                                                                   const Triangle_Projection_Info<TYPENAME>& b )
                                                             { 
                                                                if ( a.minProjection == b.minProjection )
                                                                    return a.maxProjection < b.maxProjection;
                                                                else
                                                                    return a.minProjection < b.minProjection;
                                                             }
        template <typename TYPENAME> friend inline bool operator==( const Triangle_Projection_Info<TYPENAME>& a, 
                                                                    const Triangle_Projection_Info<TYPENAME>& b )
                                                             { 
                                                                return a.minProjection == b.minProjection 
                                                                    && a.maxProjection == b.maxProjection;
                                                             }
        template <typename TYPENAME> friend inline bool operator>( const Triangle_Projection_Info<TYPENAME>& a, 
                                                                   const Triangle_Projection_Info<TYPENAME>& b )
                                                             { 
                                                                if ( a.maxProjection == b.maxProjection )
                                                                    return a.minProjection > b.minProjection;
                                                                else
                                                                    return a.maxProjection > b.maxProjection;
                                                             }
     };
     /*template <typename TYPENAME>
     class Split_OBB_Info
     {
     // Data Members
     public:
        std::vector<const GL_Triangle<TYPENAME>*> raw_triangles;
        std::vector<const GL_Triangle<TYPENAME>*> triangles;
        // The polygons with indexes less than the split_index will be split one way, 
        // and the polygons with indexes equal to and greater than the split_index will be split the other way
        size_t split_index; 
        TYPENAME weight;
     
     // Local Functions
        Split_OBB_Info( std::vector<const GL_Triangle<TYPENAME>*> raw_triangles, 
                        std::vector<const GL_Triangle<TYPENAME>*> triangles, 
                        const size_t& split_index, const TYPENAME& weight ) 
                                  : raw_triangles( raw_triangles ), triangles( triangles ), split_index( split_index ),
                                    weight( weight ){}
        Split_OBB_Info( const size_t& split_index, const TYPENAME& weight ) 
                                  : split_index( split_index ), weight( weight ){}
        Split_OBB_Info( const Split_OBB_Info<TYPENAME>& soi ) : raw_triangles( soi.raw_triangles ), 
                                                                triangles( soi.triangles ), 
                                                                split_index( soi.split_index ), weight( soi.weight ){}
        ~Split_OBB_Info(){}
        inline Split_OBB_Info<TYPENAME>& operator=( const Split_OBB_Info<TYPENAME>& soi )
             { 
                this->raw_triangles.clear();
                this->triangles.clear();
                this->raw_triangles.insert( this->raw_triangles.begin(), soi.raw_triangles.begin(), soi.raw_triangles.end() );
                this->triangles.insert( this->triangles.begin(), soi.triangles.begin(), soi.triangles.end() );
                this->split_index = soi.split_index;
                this->weight = soi.weight;
                return *this;
            }
        
     // Friend Functions
        template <typename TYPENAME> friend inline bool operator<( const Split_OBB_Info<TYPENAME>& a, 
                                                                   const Split_OBB_Info<TYPENAME>& b )
                             { 
                                if ( a.weight == b.weight )
                                {
                                    size_t median = ceil(((GLfloat)a.raw_triangles.size()) / 2.0f);
                                    return fabs((GLfloat) a.split_index - median ) > fabs((GLfloat) b.split_index - median );
                                }
                                else
                                    return a.weight < b.weight;
                             }
        template <typename TYPENAME> friend inline bool operator==( const Split_OBB_Info<TYPENAME>& a, 
                                                                    const Split_OBB_Info<TYPENAME>& b )
                                                             { 
                                                                return a.weight == b.weight 
                                                                    && a.index == b.index;
                                                             }
        template <typename TYPENAME> friend inline bool operator>( const Split_OBB_Info<TYPENAME>& a, 
                                                                   const Split_OBB_Info<TYPENAME>& b )
                             { 
                                if ( a.weight == b.weight )
                                {
                                    size_t median = ceil(((GLfloat)a.raw_triangles.size()) / 2.0f);
                                    return fabs((GLfloat) a.split_index - median ) < fabs((GLfloat) b.split_index - median );
                                }
                                else
                                    return a.weight > b.weight;
                             }
     };*/
     template <typename TYPENAME>
     class Split_OBB_Info
     {
     // Data Members
     public:
        std::vector<const GL_Triangle<TYPENAME>*> raw_triangle_Ptrs;
        std::vector<const GL_Triangle<TYPENAME>*> triangle_Ptrs;
        // The polygons with indexes less than the split_index will be split one way, 
        // and the polygons with indexes equal to and greater than the split_index will be split the other way
        size_t split_index; 
     
     // Local Functions
        Split_OBB_Info( std::vector<const GL_Triangle<TYPENAME>*>& raw_triangle_Ptrs, 
                        std::vector<const GL_Triangle<TYPENAME>*>& triangle_Ptrs, 
                        const size_t& split_index ) 
                                  : raw_triangle_Ptrs( raw_triangle_Ptrs ), 
                                    triangle_Ptrs( triangle_Ptrs ), 
                                    split_index( split_index ){}
        Split_OBB_Info( const size_t split_index = 0.0f ) 
                                  : split_index( split_index ){}
        Split_OBB_Info( const Split_OBB_Info<TYPENAME>& soi ) : raw_triangle_Ptrs( soi.raw_triangle_Ptrs ), 
                                                                triangle_Ptrs( soi.triangle_Ptrs ), 
                                                                split_index( soi.split_index ){}
        ~Split_OBB_Info(){}
        inline Split_OBB_Info<TYPENAME>& operator=( const Split_OBB_Info<TYPENAME>& soi )
             { 
                this->raw_triangle_Ptrs.assign( soi.raw_triangle_Ptrs.begin(), soi.raw_triangle_Ptrs.end() );
                this->triangle_Ptrs.assign( soi.triangle_Ptrs.begin(), soi.triangle_Ptrs.end() );
                this->split_index = soi.split_index;
                return *this;
            }
        
     // Friend Functions
        template <typename TYPENAME> friend inline bool operator<( const Split_OBB_Info<TYPENAME>& a, 
                                                                   const Split_OBB_Info<TYPENAME>& b )
                             { 
                                size_t median( ceil(((GLfloat)a.raw_triangle_Ptrs.size()) / 2.0f) );
                                return fabs( (GLfloat) a.split_index - median ) > fabs((GLfloat) b.split_index - median );
                             }
        template <typename TYPENAME> friend inline bool operator==( const Split_OBB_Info<TYPENAME>& a, 
                                                                    const Split_OBB_Info<TYPENAME>& b )
                                                             { 
                                                                return a.index == b.index;
                                                             }
        template <typename TYPENAME> friend inline bool operator>( const Split_OBB_Info<TYPENAME>& a, 
                                                                   const Split_OBB_Info<TYPENAME>& b )
                             { 
                                size_t median( ceil(((GLfloat)a.raw_triangle_Ptrs.size()) / 2.0f) );
                                return fabs((GLfloat) a.split_index - median ) < fabs((GLfloat) b.split_index - median );
                             }
     };
 };
#include "OBBTree_Test.h"
 /** LOCAL FUNCTIONS **/
 
 #if 0
 /**
  * Constructor
  * The constructor takes in a vector of raw triangle pointers and triangle pointers representing the object
  * of the specified pointer.
  * The maxDepth denotes when to stopping splitting the OBBTree into more levels. The value of the specified maxDepth
  * determines the maximum depth of the OBBTree. If maxDepth is negative, there will be no limits on the maximum depth
  * of the OBBTree, and the OBBTree will keep on splitting the OBB until each leaf node contains only one triangle.
  *
  * @param (const GL_Object<TYPENAME>* const) object
  * @param (int) maxDepth - maximum depth of the OBBTree
  * @param (const std::vector< GL_Triangle<TYPENAME>* >&) raw_triangle_Addresses
  * @param (const std::vector< GL_Triangle<TYPENAME>* >&) triangle_Addresses - pointers to current state of the raw triangles
  */
 template< typename TYPENAME >
 OBBTree<TYPENAME>::OBBTree( const GL_Object<TYPENAME>* const object, int maxDepth,
                             const std::vector< const GL_Triangle<TYPENAME>* >& raw_triangle_Addresses,
                             const std::vector< const GL_Triangle<TYPENAME>* >& triangle_Addresses )
                            : BinaryTree_Interface< OBBTree<TYPENAME> >(), 
 /*#if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
                              isMaintained( true ),
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1*/
                              obb( object ), object( object ), 
                              triangle_Ptrs( triangle_Addresses )
 {
    if ( raw_triangle_Addresses.size() > 0 )
    {
        obb = computeOBB( &raw_triangle_Addresses[0], raw_triangle_Addresses.size() );
    
        /*Matrix3<TYPENAME> orientation( covariance( triangle_Ptrs ) );
        printf("OBB Test\n");
        printf( "size: %1d\n", triangle_Ptrs.size() );
        print( orientation );
        print( orientation.getInverse() );
        print( orientation * orientation.getTranspose() );
        //Vector3<TYPENAME> vecx(orientation.Xx, orientation.Yx, orientation.Zx);
        //Vector3<TYPENAME> vecy(orientation.Xy, orientation.Yy, orientation.Zy);
        //Vector3<TYPENAME> vecz(orientation.Xz, orientation.Yz, orientation.Zz);
        Vector3<TYPENAME> vecx(orientation.Xx, orientation.Xy, orientation.Xz);
        Vector3<TYPENAME> vecy(orientation.Yx, orientation.Yy, orientation.Yz);
        Vector3<TYPENAME> vecz(orientation.Zx, orientation.Zy, orientation.Zz);
        //print( orientation );
        //print( orientation.getInverse() );
        printf("%1f\n", vecx*vecy);
        printf("%1f\n", vecx*vecz);
        printf("%1f\n", vecy*vecz);*/
        
        // calculate and split the OBBTree into a hierarchy of OBBs
        if ( raw_triangle_Addresses.size() > 1 && maxDepth != 0 )
            splitNextLevel( &raw_triangle_Addresses[0], raw_triangle_Addresses.size(), ( maxDepth > 0 ? --maxDepth : maxDepth ) );
    }
 }
 #endif
 
 /**
  * Constructor
  * The constructor takes in a reference to the object, raw triangle pointers array and triangle pointers array
  * representing the object, and the number of triangles in the arrays.
  * The maxDepth denotes when to stopping splitting the OBBTree into more levels. The value of the specified maxDepth
  * determines the maximum depth of the OBBTree. If maxDepth is negative, there will be no limits on the maximum depth
  * of the OBBTree, and the OBBTree will keep on splitting the OBB until each leaf node contains only one triangle.
  *
  * @param (const GL_Object<TYPENAME>* const) object
  * @param (size_t) maxDepth - maximum depth level of the OBBTree
  * @param (const GL_Triangle<TYPENAME>* const * const) raw_triangle_Ptrs_Ptr
  * @param (const GL_Triangle<TYPENAME>* const * const) triangle_Ptrs_Ptr - pointers to current state of the raw triangles
  * @param (size_t) size
  */
 template< typename TYPENAME >
 OBBTree<TYPENAME>::OBBTree( const GL_Object<TYPENAME>* const object, int maxDepth,
                             const GL_Triangle<TYPENAME>* const * const raw_triangle_Ptrs_Ptr, 
                             const GL_Triangle<TYPENAME>* const * const triangle_Ptrs_Ptr, size_t size )
                             : BinaryTree_Interface< OBBTree<TYPENAME> >(), 
 /*#if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
                              isMaintained( true ),
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1*/
                              obb( object ), object( object ),
                              triangle_Ptrs( triangle_Ptrs_Ptr, triangle_Ptrs_Ptr + size )
 {
    obb = computeOBB( raw_triangle_Ptrs_Ptr, size );
    
    if ( size > 1 && maxDepth != 0 )
        splitNextLevel( raw_triangle_Ptrs_Ptr, size, ( maxDepth > 0 ? --maxDepth : maxDepth ) );
 }
 
 /**
  * Alternative Constructor
  * The constructor takes in a reference to the object, raw triangles array and triangles array
  * representing the object, and the number of triangles in the arrays.
  * The maxDepth denotes when to stopping splitting the OBBTree into more levels. The value of the specified maxDepth
  * determines the maximum depth of the OBBTree. If maxDepth is negative, there will be no limits on the maximum depth
  * of the OBBTree, and the OBBTree will keep on splitting the OBB until each leaf node contains only one triangle.
  *
  * @param (const GL_Object<TYPENAME>* const) object
  * @param (size_t) maxDepth - maximum depth level of the OBBTree
  * @param (const GL_Triangle<TYPENAME>* const) raw_triangles_Ptr
  * @param (const GL_Triangle<TYPENAME>* const) triangles_Ptr - pointers to current state of the raw triangles
  * @param (size_t) size
  */
 template< typename TYPENAME >
 OBBTree<TYPENAME>::OBBTree( const GL_Object<TYPENAME>* const object, int maxDepth,
                             const GL_Triangle<TYPENAME>* const raw_triangles_Ptr, 
                             const GL_Triangle<TYPENAME>* const triangles_Ptr, size_t size )
                             : BinaryTree_Interface< OBBTree<TYPENAME> >(), 
 /*#if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
                              isMaintained( true ),
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1*/
                              obb( object ), object( object )
 {
    std::vector< const GL_Triangle<TYPENAME>* > raw_triangle_Addresses;
    for ( size_t i = 0x0; i < size; ++i )
    {
        raw_triangle_Addresses.push_back( &raw_triangles_Ptr[i] );
        triangle_Ptrs.push_back( &triangles_Ptr[i] );
    }
    
    obb = computeOBB( raw_triangles_Ptr, size );
    
    if ( size > 1 && maxDepth != 0 )
        splitNextLevel( &raw_triangle_Addresses[0], size, ( maxDepth > 0 ? --maxDepth : maxDepth ) );
 }
 
 /**
  * Alternative Constructor
  * Constructor copies the content of the specified OBBTree, but belongs to the specified object instead
  * of the object of the specified OBBTree. It is optional to specify the maximum depth of the OBBTree to 
  * copy.
  */
 template< typename TYPENAME >
 OBBTree<TYPENAME>::OBBTree( const OBBTree<TYPENAME>& obbtree, const GL_Object<TYPENAME>* const object, int maxDepth )
                        : BinaryTree_Interface< OBBTree<TYPENAME> >(),
 /*#if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
                          isMaintained( true ),
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1*/
                          obb( obbtree.obb, object ), 
                          object( object ), triangle_Ptrs()
                          
 {
    copyTriangles( obbtree );
    
    if ( maxDepth > 0 )
    {
        leftChild_Ptr = ( obbtree.constLeftChild() != NULL ? 
                                new OBBTree<TYPENAME>( *obbtree.constLeftChild(), object, --maxDepth ) : NULL );
        rightChild_Ptr = ( obbtree.constRightChild() != NULL ? 
                                new OBBTree<TYPENAME>( *obbtree.constRightChild(), object, maxDepth ) : NULL );
    }
    else if ( maxDepth < 0 )
    {
        leftChild_Ptr = ( obbtree.constLeftChild() != NULL ? 
                                new OBBTree<TYPENAME>( *obbtree.constLeftChild(), object, maxDepth ) : NULL );
        rightChild_Ptr = ( obbtree.constRightChild() != NULL ? 
                                new OBBTree<TYPENAME>( *obbtree.constRightChild(), object, maxDepth ) : NULL );
    }
    //else
    //{
    //    leftChild_Ptr = NULL;
    //    rightChild_Ptr = NULL;
    //}
 }
 
 /**
  * Destructor
  */
 template< typename TYPENAME >
 OBBTree<TYPENAME>::~OBBTree()
 {
 
 }
 
 /**
  * operator=() copies the content of the specified OBBTree to this OBBTree (except do
  * not copy the object pointer reference.
  *
  * @param (const OBBTree<TYPENAME>&) obbtree
  * @return OBBTree<TYPENAME>&
  */
 template< typename TYPENAME >
 inline OBBTree<TYPENAME>& OBBTree<TYPENAME>::operator=( const OBBTree<TYPENAME>& obbtree )
 {
 /*#if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
    isMaintained = obbtree.isMaintained;
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1*/
 
    // Delete children
    delete leftChild_Ptr;
    delete rightChild_Ptr;
 
    // Copy OBB
    obb = obbtree.obb;

    // Copy triangles relation to own object
    copyTriangles( obbtree );
    
    // Copy children
    leftChild_Ptr = ( obbtree.constLeftChild() != NULL ? new OBBTree<TYPENAME>( *obbtree.constLeftChild(), object ) : NULL );
    rightChild_Ptr = ( obbtree.constRightChild() != NULL ? new OBBTree<TYPENAME>( *obbtree.constRightChild(), object ) : NULL );
    
    return *this;
 }
 
  /** Private Functions **/
 
  /**
  * copyTriangles() has this OBBTree point to the same indexes of triangles of its
  * object as the indexes of the specified obbtree to its object.
  *
  * @param (const OBBTree<TYPENAME>&) obbtree
  */
 template< typename TYPENAME >
 inline void OBBTree<TYPENAME>::copyTriangles( const OBBTree<TYPENAME>& obbtree )
 {
    triangle_Ptrs.clear();
 
    size_t triangle_index( obbtree.triangle_Ptrs.size() );
    if ( triangle_index > 0 )
        do
        {
            --triangle_index;
            
            const GL_Triangle<TYPENAME>& triangle( *obbtree.triangle_Ptrs[ triangle_index ] );
            size_t index( obbtree.object->getTriangleIndex( triangle ) );

            triangle_Ptrs.push_back( &object->getTriangle( index ) );
        } while( triangle_index > 0 );
 }
 
 #if 0
 /**
  * computeOBB() computes and returns a tight-fitting OBB for the specified triangles of the specified object.
  *
  * @param (const std::vector< const GL_Triangle<TYPENAME>* >&) raw_triangle_Ptrs
  * @return OBB<TYPENAME>
  */
 template <typename TYPENAME>
 inline OBB<TYPENAME> OBBTree<TYPENAME>::computeOBB( const std::vector< const GL_Triangle<TYPENAME>* >& raw_triangles ) const 
 {
    // calculate half_width, half_height, half_depth, orientation for OBB
    if ( raw_triangles.size() > 0 )
    {
        ConvexHull<TYPENAME> ch( &raw_triangles[0], raw_triangles.size() );
        OrientationMatrix3<TYPENAME> orientation( covariance( &ch.triangles[0], ch.triangles.size() ) );
        
        const Vector3<TYPENAME>& xAxis = orientation.getXAxis();
        const Vector3<TYPENAME>& yAxis = orientation.getYAxis();
        const Vector3<TYPENAME>& zAxis = orientation.getZAxis();

        Vector3<TYPENAME> tA = raw_triangles[0]->getEndPointA();
        Vector3<TYPENAME> tB = raw_triangles[0]->getEndPointB();
        Vector3<TYPENAME> tC = raw_triangles[0]->getEndPointC();
        
        // A projection
        Vector3<TYPENAME> maxExtent( tA * xAxis, tA * yAxis, tA * zAxis );    // maxExtent
        Vector3<TYPENAME> minExtent( maxExtent );                             // minExtent
          
        // B projection
        findMinMaxProjectionExtent( tB, xAxis, minExtent.x, maxExtent.x );
        findMinMaxProjectionExtent( tB, yAxis, minExtent.y, maxExtent.y );
        findMinMaxProjectionExtent( tB, zAxis, minExtent.z, maxExtent.z );
            
        // C projection
        findMinMaxProjectionExtent( tC, xAxis, minExtent.x, maxExtent.x );
        findMinMaxProjectionExtent( tC, yAxis, minExtent.y, maxExtent.y );
        findMinMaxProjectionExtent( tC, zAxis, minExtent.z, maxExtent.z );
        
        size_t numberOfraw_triangles = raw_triangles.size();
        for ( size_t i = 0x1; i < numberOfraw_triangles; ++i )
        {
            tA = raw_triangles[i]->getEndPointA();
            tB = raw_triangles[i]->getEndPointB();
            tC = raw_triangles[i]->getEndPointC();
            
            // x-axis projection
            findMinMaxProjectionExtent( tA, xAxis, minExtent.x, maxExtent.x );
            findMinMaxProjectionExtent( tB, xAxis, minExtent.x, maxExtent.x );
            findMinMaxProjectionExtent( tC, xAxis, minExtent.x, maxExtent.x );
          
            // y-axis projection
            findMinMaxProjectionExtent( tA, yAxis, minExtent.y, maxExtent.y );
            findMinMaxProjectionExtent( tB, yAxis, minExtent.y, maxExtent.y );
            findMinMaxProjectionExtent( tC, yAxis, minExtent.y, maxExtent.y );
            
            // z-axis projection
            findMinMaxProjectionExtent( tA, zAxis, minExtent.z, maxExtent.z );
            findMinMaxProjectionExtent( tB, zAxis, minExtent.z, maxExtent.z );
            findMinMaxProjectionExtent( tC, zAxis, minExtent.z, maxExtent.z );
        }
        
        TYPENAME half_width = (maxExtent.x - minExtent.x) / 2.0f;
        TYPENAME half_height = (maxExtent.y - minExtent.y) / 2.0f;
        TYPENAME half_depth = (maxExtent.z - minExtent.z) / 2.0f;

        // The half-dimensions apply to the axes of the orientation (i.e. not the global axes)
        Matrix3<TYPENAME> R( orientation );
        R.Xx *= half_width;
        R.Xy *= half_width;
        R.Xz *= half_width;
        R.Yx *= half_height;
        R.Yy *= half_height;
        R.Yz *= half_height;
        R.Zx *= half_depth;
        R.Zy *= half_depth;
        R.Zz *= half_depth;

        // Rotate minExtent to the orientation position
        minExtent = orientation * minExtent;

        TYPENAME centerX = minExtent.x + R.Xx + R.Yx + R.Zx;
        TYPENAME centerY = minExtent.y + R.Xy + R.Yy + R.Zy;
        TYPENAME centerZ = minExtent.z + R.Xz + R.Yz + R.Zz;
        
 #if IMPLEMENT_BOUNDING_SPHERE == 1
        Vector3<TYPENAME> center( centerX, centerY, centerZ );
        TYPENAME radius( 0.0f );
        for ( size_t i = 0x0; i < numberOfraw_triangles; ++i )
        {
            tA = raw_triangles[i]->getEndPointA();
            tB = raw_triangles[i]->getEndPointB();
            tC = raw_triangles[i]->getEndPointC();
            
            radius = max( radius, magnitude_squared( tA - center ) );
            radius = max( radius, magnitude_squared( tB - center ) );
            radius = max( radius, magnitude_squared( tC - center ) );
        }
        radius = sqrt( radius );
        
        return OBB<TYPENAME>( object, radius, half_width, half_height, half_depth, centerX, centerY, centerZ, orientation );
 #endif // IMPLEMENT_BOUNDING_SPHERE == 1
 
 #if IMPLEMENT_BOUNDING_SPHERE != 1    
        return OBB<TYPENAME>( object, half_width, half_height, half_depth, centerX, centerY, centerZ, orientation );
 #endif // IMPLEMENT_BOUNDING_SPHERE != 1
    }
    else // no polygons exist for this OBBTree
    {
 #if IMPLEMENT_BOUNDING_SPHERE == 1
        return OBB<TYPENAME>( object, 0.0f, 0.0f, 0.0f, 0.0f, std::numeric_limits<TYPENAME>::min(), 
                             std::numeric_limits<TYPENAME>::min(), std::numeric_limits<TYPENAME>::min(), 
                             OrientationMatrix3<TYPENAME>( 0.0f, 0.0f, 0.0f, 
                                                           0.0f, 0.0f, 0.0f, 
                                                           0.0f, 0.0f, 0.0f ) );
 #endif // IMPLEMENT_BOUNDING_SPHERE == 1
 
 #if IMPLEMENT_BOUNDING_SPHERE != 1
        return OBB<TYPENAME>( object, 0.0f, 0.0f, 0.0f, std::numeric_limits<TYPENAME>::min(), 
                             std::numeric_limits<TYPENAME>::min(), std::numeric_limits<TYPENAME>::min(), 
                             OrientationMatrix3<TYPENAME>( 0.0f, 0.0f, 0.0f, 
                                                           0.0f, 0.0f, 0.0f, 
                                                           0.0f, 0.0f, 0.0f ) );
 #endif // IMPLEMENT_BOUNDING_SPHERE != 1
    }
 }
 #endif
 
 /**
  * computeOBB() computes and returns a tight-fitting OBB for the specified triangles.
  *
  * @param (const GL_Triangle<TYPENAME>* const) raw_triangles_Ptr
  * @param (const size_t) size - number of triangles
  * @return OBB<TYPENAME>
  */
 template <typename TYPENAME>
 inline OBB<TYPENAME> OBBTree<TYPENAME>::computeOBB( const GL_Triangle<TYPENAME>* const raw_triangles_Ptr, 
                                                     const size_t size ) const 
 {
    // calculate half_width, half_height, half_depth, orientation for OBB
    if ( size > 0 )
    {
        ConvexHull<TYPENAME> ch( raw_triangles_Ptr, size );
        OrientationMatrix3<TYPENAME> orientation( covariance( &ch.triangles[0], ch.triangles.size() ) );
        
        // We project the triangles of the convex hull instead of the raw triangles because if the object
        // is closed (which is most likely the case), there will always be less or equal the number of 
        // triangles for the convex hull compared to the number of raw triangles. Projecting the triangles
        // of the convex hull or the raw triangles yield the same oriented-bounding box nonetheless.
        
        const Vector3<TYPENAME>& xAxis( orientation.getXAxis() );
        const Vector3<TYPENAME>& yAxis( orientation.getYAxis() );
        const Vector3<TYPENAME>& zAxis( orientation.getZAxis() );

        Vector3<TYPENAME> tA( ch.triangles[0].getEndPointA() );
        Vector3<TYPENAME> tB( ch.triangles[0].getEndPointB() );
        Vector3<TYPENAME> tC( ch.triangles[0].getEndPointC() );
        
        // A projection
        Vector3<TYPENAME> maxExtent( tA * xAxis, tA * yAxis, tA * zAxis );    // maxExtent
        Vector3<TYPENAME> minExtent( maxExtent );                             // minExtent
          
        // B projection
        findMinMaxProjectionExtent( tB, xAxis, minExtent.x, maxExtent.x );
        findMinMaxProjectionExtent( tB, yAxis, minExtent.y, maxExtent.y );
        findMinMaxProjectionExtent( tB, zAxis, minExtent.z, maxExtent.z );
            
        // C projection
        findMinMaxProjectionExtent( tC, xAxis, minExtent.x, maxExtent.x );
        findMinMaxProjectionExtent( tC, yAxis, minExtent.y, maxExtent.y );
        findMinMaxProjectionExtent( tC, zAxis, minExtent.z, maxExtent.z );
        
        size_t i( 1 );
        while ( i < ch.triangles.size() )
        {
            tA = ch.triangles[i].getEndPointA();
            tB = ch.triangles[i].getEndPointB();
            tC = ch.triangles[i].getEndPointC();
            
            // x-axis projection
            findMinMaxProjectionExtent( tA, xAxis, minExtent.x, maxExtent.x );
            findMinMaxProjectionExtent( tB, xAxis, minExtent.x, maxExtent.x );
            findMinMaxProjectionExtent( tC, xAxis, minExtent.x, maxExtent.x );
          
            // y-axis projection
            findMinMaxProjectionExtent( tA, yAxis, minExtent.y, maxExtent.y );
            findMinMaxProjectionExtent( tB, yAxis, minExtent.y, maxExtent.y );
            findMinMaxProjectionExtent( tC, yAxis, minExtent.y, maxExtent.y );
            
            // z-axis projection
            findMinMaxProjectionExtent( tA, zAxis, minExtent.z, maxExtent.z );
            findMinMaxProjectionExtent( tB, zAxis, minExtent.z, maxExtent.z );
            findMinMaxProjectionExtent( tC, zAxis, minExtent.z, maxExtent.z );
            
            ++i;
        }
        
        const TYPENAME half_width( (maxExtent.x - minExtent.x) / 2.0f );
        const TYPENAME half_height( (maxExtent.y - minExtent.y) / 2.0f );
        const TYPENAME half_depth( (maxExtent.z - minExtent.z) / 2.0f );

        // The half-dimensions apply to the axes of the orientation (i.e. not the global axes)
        Matrix3<TYPENAME> R( orientation );
        R.Xx *= half_width;
        R.Xy *= half_width;
        R.Xz *= half_width;
        R.Yx *= half_height;
        R.Yy *= half_height;
        R.Yz *= half_height;
        R.Zx *= half_depth;
        R.Zy *= half_depth;
        R.Zz *= half_depth;

        // Rotate minExtent to the orientation position
        minExtent = orientation * minExtent;

        const TYPENAME centerX( minExtent.x + R.Xx + R.Yx + R.Zx );
        const TYPENAME centerY( minExtent.y + R.Xy + R.Yy + R.Zy );
        const TYPENAME centerZ( minExtent.z + R.Xz + R.Yz + R.Zz );
        
 #if IMPLEMENT_BOUNDING_SPHERE == 1
        Vector3<TYPENAME> center( centerX, centerY, centerZ );
        TYPENAME radius( 0.0f );
        i = 0;
        while ( i < ch.triangles.size() )
        {
            tA = ch.triangles[i].getEndPointA();
            tB = ch.triangles[i].getEndPointB();
            tC = ch.triangles[i].getEndPointC();
            
            const TYPENAME magA( magnitude_squared( tA - center ) );
            const TYPENAME magB( magnitude_squared( tB - center ) );
            const TYPENAME magC( magnitude_squared( tC - center ) );
            
            radius = MAX( radius, magA );
            radius = MAX( radius, magB );
            radius = MAX( radius, magC );
            
            ++i;
        }
        radius = sqrt( radius );
        
        return OBB<TYPENAME>( object, radius, half_width, half_height, half_depth, centerX, centerY, centerZ, orientation );
 #endif // IMPLEMENT_BOUNDING_SPHERE == 1
 
 #if IMPLEMENT_BOUNDING_SPHERE != 1    
        return OBB<TYPENAME>( object, half_width, half_height, half_depth, centerX, centerY, centerZ, orientation );
 #endif // IMPLEMENT_BOUNDING_SPHERE != 1
    }
    else // no polygons exist for this OBBTree
    {
 #if IMPLEMENT_BOUNDING_SPHERE == 1
        return OBB<TYPENAME>( object, 0.0f, 0.0f, 0.0f, 0.0f, std::numeric_limits<TYPENAME>::min(), 
                             std::numeric_limits<TYPENAME>::min(), std::numeric_limits<TYPENAME>::min(), 
                             OrientationMatrix3<TYPENAME>( 0.0f, 0.0f, 0.0f, 
                                                           0.0f, 0.0f, 0.0f, 
                                                           0.0f, 0.0f, 0.0f ) );
 #endif // IMPLEMENT_BOUNDING_SPHERE == 1
 
 #if IMPLEMENT_BOUNDING_SPHERE != 1
        return OBB<TYPENAME>( object, 0.0f, 0.0f, 0.0f, std::numeric_limits<TYPENAME>::min(), 
                             std::numeric_limits<TYPENAME>::min(), std::numeric_limits<TYPENAME>::min(), 
                             OrientationMatrix3<TYPENAME>( 0.0f, 0.0f, 0.0f, 
                                                           0.0f, 0.0f, 0.0f, 
                                                           0.0f, 0.0f, 0.0f ) );
 #endif // IMPLEMENT_BOUNDING_SPHERE != 1
    }
 }
 
 /**
  * computeOBB() computes and returns a tight-fitting OBB for the specified triangles.
  *
  * @param (const GL_Triangle<TYPENAME>* const * const) raw_triangle_Ptrs_Ptr
  * @param (const size_t) size - number of triangles
  * @return OBB<TYPENAME>
  */
 template <typename TYPENAME>
 inline OBB<TYPENAME> OBBTree<TYPENAME>::computeOBB( const GL_Triangle<TYPENAME>* const * const raw_triangle_Ptrs_Ptr, 
                                                     const size_t size ) const
 {
    // calculate half_width, half_height, half_depth, orientation for OBB
    if ( size > 0 )
    {
        ConvexHull<TYPENAME> ch( raw_triangle_Ptrs_Ptr, size );
        OrientationMatrix3<TYPENAME> orientation( covariance( &ch.triangles[0], ch.triangles.size() ) );
        
        // We project the triangles of the convex hull instead of the raw triangles because if the object
        // is closed (which is most likely the case), there will always be less or equal the number of 
        // triangles for the convex hull compared to the number of raw triangles. Projecting the triangles
        // of the convex hull or the raw triangles yield the same oriented-bounding box nonetheless.
        
        const Vector3<TYPENAME>& xAxis = orientation.getXAxis();
        const Vector3<TYPENAME>& yAxis = orientation.getYAxis();
        const Vector3<TYPENAME>& zAxis = orientation.getZAxis();

        Vector3<TYPENAME> tA( ch.triangles[0].getEndPointA() );
        Vector3<TYPENAME> tB( ch.triangles[0].getEndPointB() );
        Vector3<TYPENAME> tC( ch.triangles[0].getEndPointC() );
        
        // A projection
        Vector3<TYPENAME> maxExtent( tA * xAxis, tA * yAxis, tA * zAxis );    // maxExtent
        Vector3<TYPENAME> minExtent( maxExtent );                             // minExtent
          
        // B projection
        findMinMaxProjectionExtent( tB, xAxis, minExtent.x, maxExtent.x );
        findMinMaxProjectionExtent( tB, yAxis, minExtent.y, maxExtent.y );
        findMinMaxProjectionExtent( tB, zAxis, minExtent.z, maxExtent.z );
            
        // C projection
        findMinMaxProjectionExtent( tC, xAxis, minExtent.x, maxExtent.x );
        findMinMaxProjectionExtent( tC, yAxis, minExtent.y, maxExtent.y );
        findMinMaxProjectionExtent( tC, zAxis, minExtent.z, maxExtent.z );
        
        size_t i( 1 );
        while ( i < ch.triangles.size() )
        {
            tA = ch.triangles[i].getEndPointA();
            tB = ch.triangles[i].getEndPointB();
            tC = ch.triangles[i].getEndPointC();
            
            // x-axis projection
            findMinMaxProjectionExtent( tA, xAxis, minExtent.x, maxExtent.x );
            findMinMaxProjectionExtent( tB, xAxis, minExtent.x, maxExtent.x );
            findMinMaxProjectionExtent( tC, xAxis, minExtent.x, maxExtent.x );
          
            // y-axis projection
            findMinMaxProjectionExtent( tA, yAxis, minExtent.y, maxExtent.y );
            findMinMaxProjectionExtent( tB, yAxis, minExtent.y, maxExtent.y );
            findMinMaxProjectionExtent( tC, yAxis, minExtent.y, maxExtent.y );
            
            // z-axis projection
            findMinMaxProjectionExtent( tA, zAxis, minExtent.z, maxExtent.z );
            findMinMaxProjectionExtent( tB, zAxis, minExtent.z, maxExtent.z );
            findMinMaxProjectionExtent( tC, zAxis, minExtent.z, maxExtent.z );
            
            ++i;
        }
        
        const TYPENAME half_width( (maxExtent.x - minExtent.x) / 2.0f );
        const TYPENAME half_height( (maxExtent.y - minExtent.y) / 2.0f );
        const TYPENAME half_depth( (maxExtent.z - minExtent.z) / 2.0f );

        // The half-dimensions apply to the axes of the orientation (i.e. not the global axes)
        Matrix3<TYPENAME> R( orientation );
        R.Xx *= half_width;
        R.Xy *= half_width;
        R.Xz *= half_width;
        R.Yx *= half_height;
        R.Yy *= half_height;
        R.Yz *= half_height;
        R.Zx *= half_depth;
        R.Zy *= half_depth;
        R.Zz *= half_depth;

        // Rotate minExtent to the orientation position
        minExtent = orientation * minExtent;

        const TYPENAME centerX( minExtent.x + R.Xx + R.Yx + R.Zx );
        const TYPENAME centerY( minExtent.y + R.Xy + R.Yy + R.Zy );
        const TYPENAME centerZ( minExtent.z + R.Xz + R.Yz + R.Zz );
        
 #if IMPLEMENT_BOUNDING_SPHERE == 1
        Vector3<TYPENAME> center( centerX, centerY, centerZ );
        TYPENAME radius( 0.0f );
        i = 0;
        while ( i < ch.triangles.size() )
        {
            tA = ch.triangles[i].getEndPointA();
            tB = ch.triangles[i].getEndPointB();
            tC = ch.triangles[i].getEndPointC();
            
            const TYPENAME magA( magnitude_squared( tA - center ) );
            const TYPENAME magB( magnitude_squared( tB - center ) );
            const TYPENAME magC( magnitude_squared( tC - center ) );
            
            radius = MAX( radius, magA );
            radius = MAX( radius, magB );
            radius = MAX( radius, magC );
            
            ++i;
        }
        radius = sqrt( radius );
        
        return OBB<TYPENAME>( object, radius, half_width, half_height, half_depth, centerX, centerY, centerZ, orientation );
 #endif // IMPLEMENT_BOUNDING_SPHERE == 1
 
 #if IMPLEMENT_BOUNDING_SPHERE != 1    
        return OBB<TYPENAME>( object, half_width, half_height, half_depth, centerX, centerY, centerZ, orientation );
 #endif // IMPLEMENT_BOUNDING_SPHERE != 1
    }
    else // no polygons exist for this OBBTree
    {
 #if IMPLEMENT_BOUNDING_SPHERE == 1
        return OBB<TYPENAME>( object, 0.0f, 0.0f, 0.0f, 0.0f, std::numeric_limits<TYPENAME>::min(), 
                             std::numeric_limits<TYPENAME>::min(), std::numeric_limits<TYPENAME>::min(), 
                             OrientationMatrix3<TYPENAME>( 0.0f, 0.0f, 0.0f, 
                                                           0.0f, 0.0f, 0.0f, 
                                                           0.0f, 0.0f, 0.0f ) );
 #endif // IMPLEMENT_BOUNDING_SPHERE == 1
 
 #if IMPLEMENT_BOUNDING_SPHERE != 1
        return OBB<TYPENAME>( object, 0.0f, 0.0f, 0.0f, std::numeric_limits<TYPENAME>::min(), 
                             std::numeric_limits<TYPENAME>::min(), std::numeric_limits<TYPENAME>::min(), 
                             OrientationMatrix3<TYPENAME>( 0.0f, 0.0f, 0.0f, 
                                                           0.0f, 0.0f, 0.0f, 
                                                           0.0f, 0.0f, 0.0f ) );
 #endif // IMPLEMENT_BOUNDING_SPHERE != 1
    }
 }
 
 /** 
  * covariance() returns the normalized covariance matrix for the group of triangles (i.e. std::vector of triangle pointers).
  *
  * The covariance matrix represents the axes of the OBB after normalizing the covariance matrix axes.
  *
  *     [ Cxx Cyx Czx 
  * C =   Cxy Cyy Czy
  * 	  Cxz Cyz Czz ]
  *
  * @param (const std::vector<const GL_Triangle<TYPENAME> * const>) triangles
  * @return Matrix3<TYPENAME>
  */
 /*template <typename TYPENAME>
 inline Matrix3<TYPENAME> OBBTree<TYPENAME>::covariance( std::vector< const GL_Triangle<TYPENAME> * > triangles ) const 
 {
    // number of triangles
    GLint size = triangles.size();
 
    // triangles is a vector of pointers to GL_Triangle objects
    // thus, an iterator for triangles is a pointer to a pointer of a GL_Triangle object

    // n = number of triangles
    // mu = (1 / (3n)) * sum( ith_a + ith_b + ith_c )
    Vector3<TYPENAME> mu; //center( triangles ) );//( 0.0f, 0.0f, 0.0f );
    
    // n = number of triangles
    // centroid = (1 / (3n)) * sum( ith_a + ith_b + ith_c )
    Vector3<TYPENAME> centroid( 0.0f, 0.0f, 0.0f );

    // WARNING: Changed here after removing const
    //std::vector<const GL_Triangle<TYPENAME> * const>::const_iterator itr;
    std::vector< const GL_Triangle<TYPENAME>* >::const_iterator itr;
    for ( itr = triangles.begin(); itr != triangles.end(); ++itr )
	{
	    const GL_Triangle<TYPENAME> * const & triangle_Ptr = *itr;
	    
	    // sum( ith_a + ith_b + ith_c )
	    centroid += triangle_Ptr->getEndPointA() + triangle_Ptr->getEndPointB() + triangle_Ptr->getEndPointC();
	}
	// centroid = (1 / (3n)) * sum( ith_a + ith_b + ith_c )
	centroid /= 0x3*triangles.size();
	mu = centroid;
	
	

    // WARNING: Changed here after removing const
    //std::vector<const GL_Triangle<TYPENAME> * const>::const_iterator itr;
    //std::vector< const GL_Triangle<TYPENAME> *>::const_iterator itr;
    //for ( itr = triangles.begin(); itr != triangles.end(); ++itr )
	//{
	    //const GL_Triangle<TYPENAME> * const & triangle_Ptr = *itr;
	    
	    // sum( ith_a + ith_b + ith_c )
	    //mu += triangle_Ptr->getEndPointA() + triangle_Ptr->getEndPointB() + triangle_Ptr->getEndPointC();
	//}
	// mu = (1 / (3n)) * sum( ith_a + ith_b + ith_c )
	//mu /= 0x3*size;
	
	
	// Covariance Matrix C
	// Let
	// a = triangle_Ptr->a = first endpoint of the triangle
	// b = triangle_Ptr->b = second endpoint of the triangle
	// c = triangle_Ptr->c = third endpoint of the triangle
	// Pa = a - mu;
	// Pb = b - mu;
	// Pc = c - mu;
	// Cjk = (1 / (3n)) * sum( ith_Pa_j*ith_Pa_k + ith_Pb_j*ith_Pb_k + ith_Pc_j*ith_Pc_k )
	
	// covariance matrix
	Matrix3<TYPENAME> C( 0.0f, 0.0f, 0.0f,
	                     0.0f, 0.0f, 0.0f,
	                     0.0f, 0.0f, 0.0f );
	
	for ( itr = triangles.begin(); itr != triangles.end(); ++itr )
	{
	    const GL_Triangle<TYPENAME> * const & triangle_Ptr = *itr;
	    const Vector3<TYPENAME> Pa( triangle_Ptr->getEndPointA() - mu );   // endpoint a - mu
	    const Vector3<TYPENAME> Pb( triangle_Ptr->getEndPointB() - mu );   // endpoint b - mu
	    const Vector3<TYPENAME> Pc( triangle_Ptr->getEndPointC() - mu );   // endpoint c - mu
	    //printf("Test mu: \n");
	    //print( mu );
	    //print( triangle_Ptr->a );
	    //print( Pa );
	    //print( triangle_Ptr->b );
	    //print( Pb );
	    //print( triangle_Ptr->c );
	    //print( Pc );
	    //system("PAUSE");
	    
	    // sum( ith_Pa_j*ith_Pa_k + ith_Pb_j*ith_Pb_k + ith_Pc_j*ith_Pc_k )
	    C.Xx += (Pa.x*Pa.x) + (Pb.x*Pb.x) + (Pc.x*Pc.x);
	    C.Xy += (Pa.x*Pa.y) + (Pb.x*Pb.y) + (Pc.x*Pc.y);
	    C.Xz += (Pa.x*Pa.z) + (Pb.x*Pb.z) + (Pc.x*Pc.z);
	    C.Yx += (Pa.y*Pa.x) + (Pb.y*Pb.x) + (Pc.y*Pc.x);
	    C.Yy += (Pa.y*Pa.y) + (Pb.y*Pb.y) + (Pc.y*Pc.y);
	    C.Yz += (Pa.y*Pa.z) + (Pb.y*Pb.z) + (Pc.y*Pc.z);
	    C.Zx += (Pa.z*Pa.x) + (Pb.z*Pb.x) + (Pc.z*Pc.x);
	    C.Zy += (Pa.z*Pa.y) + (Pb.z*Pb.y) + (Pc.z*Pc.y);
	    C.Zz += (Pa.z*Pa.z) + (Pb.z*Pb.z) + (Pc.z*Pc.z);
	    //print( Pa );
	    //print( Pb );
	    //print( Pc );
	    //print( Vector3<TYPENAME>( C.Xx, C.Yy, C.Zz ) );
	    //system("PAUSE");
	}
	
	// Cjk = (1 / (3n)) * sum( ith_Pa_j*ith_Pa_k + ith_Pb_j*ith_Pb_k + ith_Pc_j*ith_Pc_k )
	C /= 0x3*size;
    //printf("GL_Triangle - covariance not normalized yet\n");
    
    if ( !(C.Xx || C.Xy || C.Xz) )
    {
        Vector3<TYPENAME> xAxis = Vector::crossProduct( C.Yx, C.Yy, C.Yz, C.Zx, C.Zy, C.Zz );
        
        C.Xx = xAxis.x;
        C.Xy = xAxis.y;
        C.Xz = xAxis.z;
    }
    else if ( !(C.Yx || C.Yy || C.Yz) )
    {
        Vector3<TYPENAME> yAxis = Vector::crossProduct( C.Zx, C.Zy, C.Zz, C.Xx, C.Xy, C.Xz );
        
        C.Yx = yAxis.x;
        C.Yy = yAxis.y;
        C.Yz = yAxis.z;
    }
    else if ( !(C.Zx || C.Zy || C.Zz) )
    {
        Vector3<TYPENAME> zAxis = Vector::crossProduct( C.Xx, C.Xy, C.Xz, C.Yx, C.Yy, C.Yz );
        
        C.Zx = zAxis.x;
        C.Zy = zAxis.y;
        C.Zz = zAxis.z;
    }
    Vector::normalize( C.Xx, C.Xy, C.Xz );
    Vector::normalize( C.Yx, C.Yy, C.Yz );
    Vector::normalize( C.Zx, C.Zy, C.Zz );
    
	return C;
 }*/
 
 /** 
  * covariance() returns the normalized covariance matrix for the group of triangles (i.e. std::vector of triangle pointers).
  *
  * The covariance matrix represents the axes of the OBB after normalizing the covariance matrix axes.
  *
  *     [ Cxx Cyx Czx 
  * C =   Cxy Cyy Czy
  * 	  Cxz Cyz Czz ]
  *
  * @param (const std::vector<const GL_Triangle<TYPENAME> * const>) triangles
  * @return Matrix3<TYPENAME>
  */
 /*template <typename TYPENAME>
 inline Matrix3<TYPENAME> OBBTree<TYPENAME>::covariance( const GL_Triangle<TYPENAME> * const triangles_Ptr, 
                                                                                       const size_t size ) const 
 {
    // n = number of triangles
    // mu = (1 / (3n)) * sum( ith_a + ith_b + ith_c )
    Vector3<TYPENAME> mu; //center( triangles ) );//( 0.0f, 0.0f, 0.0f );
    
    // n = number of triangles
    // centroid = (1 / (3n)) * sum( ith_a + ith_b + ith_c )
    Vector3<TYPENAME> centroid( 0.0f, 0.0f, 0.0f );
    
    size_t i( 0 );
    while ( i < size )
	{
	    // sum( ith_a + ith_b + ith_c )
	    centroid += triangles_Ptr[i].getEndPointA() + triangles_Ptr[i].getEndPointB() + triangles_Ptr[i].getEndPointC();
	    
	    ++i;
	}
	// centroid = (1 / (3n)) * sum( ith_a + ith_b + ith_c )
	centroid /= 0x3*size;
	mu = centroid;
	
	// Covariance Matrix C
	// Let
	// a = triangle_Ptr->a = first endpoint of the triangle
	// b = triangle_Ptr->b = second endpoint of the triangle
	// c = triangle_Ptr->c = third endpoint of the triangle
	// Pa = a - mu;
	// Pb = b - mu;
	// Pc = c - mu;
	// Cjk = (1 / (3n)) * sum( ith_Pa_j*ith_Pa_k + ith_Pb_j*ith_Pb_k + ith_Pc_j*ith_Pc_k )
	
	// covariance matrix
	Matrix3<TYPENAME> C( 0.0f, 0.0f, 0.0f,
	                     0.0f, 0.0f, 0.0f,
	                     0.0f, 0.0f, 0.0f );
	
	i = 0;
	while ( i < size )
	{
	    const Vector3<TYPENAME> Pa( triangles_Ptr[i].getEndPointA() - mu );   // endpoint a - mu
	    const Vector3<TYPENAME> Pb( triangles_Ptr[i].getEndPointB() - mu );   // endpoint b - mu
	    const Vector3<TYPENAME> Pc( triangles_Ptr[i].getEndPointC() - mu );   // endpoint c - mu
	    
	    // sum( ith_Pa_j*ith_Pa_k + ith_Pb_j*ith_Pb_k + ith_Pc_j*ith_Pc_k )
	    C.Xx += (Pa.x*Pa.x) + (Pb.x*Pb.x) + (Pc.x*Pc.x);
	    C.Xy += (Pa.x*Pa.y) + (Pb.x*Pb.y) + (Pc.x*Pc.y);
	    C.Xz += (Pa.x*Pa.z) + (Pb.x*Pb.z) + (Pc.x*Pc.z);
	    C.Yx += (Pa.y*Pa.x) + (Pb.y*Pb.x) + (Pc.y*Pc.x);
	    C.Yy += (Pa.y*Pa.y) + (Pb.y*Pb.y) + (Pc.y*Pc.y);
	    C.Yz += (Pa.y*Pa.z) + (Pb.y*Pb.z) + (Pc.y*Pc.z);
	    C.Zx += (Pa.z*Pa.x) + (Pb.z*Pb.x) + (Pc.z*Pc.x);
	    C.Zy += (Pa.z*Pa.y) + (Pb.z*Pb.y) + (Pc.z*Pc.y);
	    C.Zz += (Pa.z*Pa.z) + (Pb.z*Pb.z) + (Pc.z*Pc.z);
	    
	    ++i;
	}
	
	// Cjk = (1 / (3n)) * sum( ith_Pa_j*ith_Pa_k + ith_Pb_j*ith_Pb_k + ith_Pc_j*ith_Pc_k )
	C /= 0x3*size;
    
    if ( !(C.Xx || C.Xy || C.Xz) )
    {
        Vector3<TYPENAME> xAxis = Vector::crossProduct( C.Yx, C.Yy, C.Yz, C.Zx, C.Zy, C.Zz );
        
        C.Xx = xAxis.x;
        C.Xy = xAxis.y;
        C.Xz = xAxis.z;
    }
    else if ( !(C.Yx || C.Yy || C.Yz) )
    {
        Vector3<TYPENAME> yAxis = Vector::crossProduct( C.Zx, C.Zy, C.Zz, C.Xx, C.Xy, C.Xz );
        
        C.Yx = yAxis.x;
        C.Yy = yAxis.y;
        C.Yz = yAxis.z;
    }
    else if ( !(C.Zx || C.Zy || C.Zz) )
    {
        Vector3<TYPENAME> zAxis = Vector::crossProduct( C.Xx, C.Xy, C.Xz, C.Yx, C.Yy, C.Yz );
        
        C.Zx = zAxis.x;
        C.Zy = zAxis.y;
        C.Zz = zAxis.z;
    }
    Vector::normalize( C.Xx, C.Xy, C.Xz );
    Vector::normalize( C.Yx, C.Yy, C.Yz );
    Vector::normalize( C.Zx, C.Zy, C.Zz );
    
	return C;
 }*/
 
 /** WARNING: Supposed to only have triangles forming the convex hull of the object. This needs to be implemented.
  * covariance() returns the normalized covariance matrix for the group of triangles (i.e. std::vector of triangle pointers).
  *
  * The covariance matrix represents the axes of the OBB after normalizing the covariance matrix axes.
  *
  *     [ Cxx Cyx Czx 
  * C =   Cxy Cyy Czy
  * 	  Cxz Cyz Czz ]
  *
  * @param (const std::vector<const GL_Triangle<TYPENAME> * const>) triangles
  * @return Matrix3<TYPENAME>
  */
 /*template <typename TYPENAME>
 inline Matrix3<TYPENAME> OBBTree<TYPENAME>::covariance( std::vector< const GL_Triangle<TYPENAME> * > triangles ) const //const std::vector<const GL_Triangle<TYPENAME> * const> triangles )
 {
    // number of triangles
    GLint size = triangles.size();
    // cumulative area
    TYPENAME sum_area = 0.0f;
    // centroid of convex hull (if the triangles form a convex hull)
    Vector3<TYPENAME> centroid( 0.0f, 0.0f, 0.0f );
 
    // triangles is a vector of pointers to GL_Triangle objects
    // thus, an iterator for triangles is a pointer to a pointer of a GL_Triangle object

    // WARNING: Changed here after removing const
    //std::vector<const GL_Triangle<TYPENAME> * const>::const_iterator itr;
    std::vector<const GL_Triangle<TYPENAME> *>::const_iterator itr;
    for ( itr = triangles.begin(); itr != triangles.end(); ++itr )
	{
	    const GL_Triangle<TYPENAME> * const & triangle_Ptr = *itr;
	    
	    // sum = summation for all triangles
        // summation of area of all triangles
	    sum_area += area( *triangle_Ptr );
	    
	    // centroid of convex hull
	    // centroid = sum( ith_triangle_area * ith_triangle_center ) / sum( jth_triangle_area )

	    // sum( ith_triangle_area * ith_triangle_center )
	    centroid += area( *triangle_Ptr ) * center( *triangle_Ptr );
	}
	// centroid = sum( ith_triangle_area * ith_triangle_center ) / sum( jth_triangle_area )
	centroid /= sum_area;
	
	
	// Covariance Matrix C
	// Let
	// a = triangle_Ptr->a = first endpoint of the triangle
	// b = triangle_Ptr->b = second endpoint of the triangle
	// c = triangle_Ptr->c = third endpoint of the triangle
	// Cjk = sum( [(ith_triangle_area / (12*sum_area))
	//                  * (9*ith_centroid_j*ith_centroid_k + ith_a_j*ith_a_k + ith_b_j*ith_b_k + ith_c_j*ith_c_k)]
	//                  - (centroid_j*centroid_k)
	// Cjk = [(1/(12*sum_area)) 
	//   * sum( ith_triangle_area * (9*ith_centroid_j*ith_centroid_k + ith_a_j*ith_a_k + ith_b_j*ith_b_k + ith_c_j*ith_c_k) )]
	//   - (numberOfTriangles*centroid_j*centroid_k)
	
	// covariance matrix
	Matrix3<TYPENAME> C( 0.0f, 0.0f, 0.0f,
	                     0.0f, 0.0f, 0.0f,
	                     0.0f, 0.0f, 0.0f );
	
	for ( itr = triangles.begin(); itr != triangles.end(); ++itr )
	{
	    const GL_Triangle<TYPENAME> * const & triangle_Ptr = *itr;
	    const TYPENAME& Area = area( *triangle_Ptr );  // ith triangle area
	    const Vector3<TYPENAME>& Center( center( *triangle_Ptr ) ); // ith triangle center
	    const Vector3<TYPENAME>& a( triangle_Ptr->getEndPointA() );   // endpoint a
	    const Vector3<TYPENAME>& b( triangle_Ptr->getEndPointB() );   // endpoint b
	    const Vector3<TYPENAME>& c( triangle_Ptr->getEndPointC() );   // endpoint c
	    
	    // sum( ith_triangle_area * (9*ith_centroid_j*ith_centroid_k + ith_a_j*ith_a_k + ith_b_j*ith_b_k + ith_c_j*ith_c_k) )
	    C.Xx += Area * ((9.0f*Center.x*Center.x) + (a.x*a.x) + (b.x*b.x) + (c.x*c.x));
	    C.Xy += Area * ((9.0f*Center.x*Center.y) + (a.x*a.y) + (b.x*b.y) + (c.x*c.y));
	    C.Xz += Area * ((9.0f*Center.x*Center.z) + (a.x*a.z) + (b.x*b.z) + (c.x*c.z));
	    C.Yx += Area * ((9.0f*Center.y*Center.x) + (a.y*a.x) + (b.y*b.x) + (c.y*c.x));
	    C.Yy += Area * ((9.0f*Center.y*Center.y) + (a.y*a.y) + (b.y*b.y) + (c.y*c.y));
	    C.Yz += Area * ((9.0f*Center.y*Center.z) + (a.y*a.z) + (b.y*b.z) + (c.y*c.z));
	    C.Zx += Area * ((9.0f*Center.z*Center.x) + (a.z*a.x) + (b.z*b.x) + (c.z*c.x));
	    C.Zy += Area * ((9.0f*Center.z*Center.y) + (a.z*a.y) + (b.z*b.y) + (c.z*c.y));
	    C.Zz += Area * ((9.0f*Center.z*Center.z) + (a.z*a.z) + (b.z*b.z) + (c.z*c.z));
	}
	
	// Cjk = [(1/(12*sum_area)) 
	//   * sum( ith_triangle_area * (9*ith_centroid_j*ith_centroid_k + ith_a_j*ith_a_k + ith_b_j*ith_b_k + ith_c_j*ith_c_k) )]
	//   - (numberOfTriangles*centroid_j*centroid_k)
	C.Xx = (C.Xx / (12.0f*sum_area)) - (size*centroid.x*centroid.x);
	C.Xy = (C.Xy / (12.0f*sum_area)) - (size*centroid.x*centroid.y);
	C.Xz = (C.Xz / (12.0f*sum_area)) - (size*centroid.x*centroid.z);
	C.Yx = (C.Yx / (12.0f*sum_area)) - (size*centroid.y*centroid.x);
	C.Yy = (C.Yy / (12.0f*sum_area)) - (size*centroid.y*centroid.y);
	C.Yz = (C.Yz / (12.0f*sum_area)) - (size*centroid.y*centroid.z);
	C.Zx = (C.Zx / (12.0f*sum_area)) - (size*centroid.z*centroid.x);
	C.Zy = (C.Zy / (12.0f*sum_area)) - (size*centroid.z*centroid.y);
	C.Zz = (C.Zz / (12.0f*sum_area)) - (size*centroid.z*centroid.z);
	
	if ( !(C.Xx || C.Xy || C.Xz) )
    {
        Vector3<TYPENAME> xAxis = Vector::crossProduct( C.Yx, C.Yy, C.Yz, C.Zx, C.Zy, C.Zz );
        
        C.Xx = xAxis.x;
        C.Xy = xAxis.y;
        C.Xz = xAxis.z;
    }
    else if ( !(C.Yx || C.Yy || C.Yz) )
    {
        Vector3<TYPENAME> yAxis = Vector::crossProduct( C.Zx, C.Zy, C.Zz, C.Xx, C.Xy, C.Xz );
        
        C.Yx = yAxis.x;
        C.Yy = yAxis.y;
        C.Yz = yAxis.z;
    }
    else if ( !(C.Zx || C.Zy || C.Zz) )
    {
        Vector3<TYPENAME> zAxis = Vector::crossProduct( C.Xx, C.Xy, C.Xz, C.Yx, C.Yy, C.Yz );
        
        C.Zx = zAxis.x;
        C.Zy = zAxis.y;
        C.Zz = zAxis.z;
    }  
    Vector::normalize( C.Xx, C.Xy, C.Xz );
    Vector::normalize( C.Yx, C.Yy, C.Yz );
    Vector::normalize( C.Zx, C.Zy, C.Zz );
    
	return C;
 }*/
 
 /**
  * covariance() returns the normalized covariance matrix for the group of triangles (i.e. std::vector of triangle pointers).
  *
  * The covariance matrix represents the axes of the OBB after normalizing the covariance matrix axes.
  *
  *     [ Cxx Cyx Czx 
  * C =   Cxy Cyy Czy
  * 	  Cxz Cyz Czz ]
  *
  * @param (const GL_Triangle<TYPENAME> * const) triangles_Ptr - pointer to triangles
  * @param (const size_t) size - number of triangles
  * @return Matrix3<TYPENAME>
  */
 template <typename TYPENAME>
 inline Matrix3<TYPENAME> OBBTree<TYPENAME>::covariance( const GL_Triangle<TYPENAME> * const triangles_Ptr, 
                                                                                       const size_t size ) const
 {
    // cumulative area
    TYPENAME sum_area( 0.0f );
    // centroid of convex hull (if the triangles form a convex hull)
    Vector3<TYPENAME> centroid( 0.0f, 0.0f, 0.0f );
    
    // for each triangle
    size_t triangle_index( size );
    if ( size > 0 )
        do
	    {
	        --triangle_index;
	        
	        // sum = summation for all triangles
            // summation of area of all triangles
	        sum_area += area( triangles_Ptr[ triangle_index ] );
    	    
	        // centroid of convex hull
	        // centroid = sum( ith_triangle_area * ith_triangle_center ) / sum( jth_triangle_area )

	        // sum( ith_triangle_area * ith_triangle_center )
	        centroid += area( triangles_Ptr[ triangle_index ] ) * center( triangles_Ptr[ triangle_index ] );
	    } while ( triangle_index > 0 );
	// centroid = sum( ith_triangle_area * ith_triangle_center ) / sum( jth_triangle_area )
	centroid /= sum_area;
	
	
	// Covariance Matrix C
	// Let
	// a = triangle_Ptr->a = first endpoint of the triangle
	// b = triangle_Ptr->b = second endpoint of the triangle
	// c = triangle_Ptr->c = third endpoint of the triangle
	// Cjk = sum( [(ith_triangle_area / (12*sum_area))
	//                  * (9*ith_centroid_j*ith_centroid_k + ith_a_j*ith_a_k + ith_b_j*ith_b_k + ith_c_j*ith_c_k)]
	//                  - (centroid_j*centroid_k)
	// Cjk = [(1/(12*sum_area)) 
	//   * sum( ith_triangle_area * (9*ith_centroid_j*ith_centroid_k + ith_a_j*ith_a_k + ith_b_j*ith_b_k + ith_c_j*ith_c_k) )]
	//   - (numberOfTriangles*centroid_j*centroid_k)
	
	// covariance matrix
	Matrix3<TYPENAME> C( 0.0f, 0.0f, 0.0f,
	                     0.0f, 0.0f, 0.0f,
	                     0.0f, 0.0f, 0.0f );
	
	triangle_index = size;
	if ( triangle_index > 0 )
	    do
	    {
	        --triangle_index;
	        const GL_Triangle<TYPENAME>& triangle( triangles_Ptr[ triangle_index ] );
	        
	        const TYPENAME& Area = area( triangle );                // ith triangle area
	        const Vector3<TYPENAME>& Center( center( triangle ) );  // ith triangle center
	        const Vector3<TYPENAME>& a( triangle.getEndPointA() );  // endpoint a
	        const Vector3<TYPENAME>& b( triangle.getEndPointB() );  // endpoint b
	        const Vector3<TYPENAME>& c( triangle.getEndPointC() );  // endpoint c
    	    
	        // sum( ith_triangle_area * (9*ith_centroid_j*ith_centroid_k + ith_a_j*ith_a_k + ith_b_j*ith_b_k + ith_c_j*ith_c_k) )
	        C.Xx += Area * ((9.0f*Center.x*Center.x) + (a.x*a.x) + (b.x*b.x) + (c.x*c.x));
	        C.Xy += Area * ((9.0f*Center.x*Center.y) + (a.x*a.y) + (b.x*b.y) + (c.x*c.y));
	        C.Xz += Area * ((9.0f*Center.x*Center.z) + (a.x*a.z) + (b.x*b.z) + (c.x*c.z));
	        C.Yx += Area * ((9.0f*Center.y*Center.x) + (a.y*a.x) + (b.y*b.x) + (c.y*c.x));
	        C.Yy += Area * ((9.0f*Center.y*Center.y) + (a.y*a.y) + (b.y*b.y) + (c.y*c.y));
	        C.Yz += Area * ((9.0f*Center.y*Center.z) + (a.y*a.z) + (b.y*b.z) + (c.y*c.z));
	        C.Zx += Area * ((9.0f*Center.z*Center.x) + (a.z*a.x) + (b.z*b.x) + (c.z*c.x));
	        C.Zy += Area * ((9.0f*Center.z*Center.y) + (a.z*a.y) + (b.z*b.y) + (c.z*c.y));
	        C.Zz += Area * ((9.0f*Center.z*Center.z) + (a.z*a.z) + (b.z*b.z) + (c.z*c.z));
	    } while ( triangle_index > 0 );
	
	// Cjk = [(1/(12*sum_area)) 
	//   * sum( ith_triangle_area * (9*ith_centroid_j*ith_centroid_k + ith_a_j*ith_a_k + ith_b_j*ith_b_k + ith_c_j*ith_c_k) )]
	//   - (numberOfTriangles*centroid_j*centroid_k)
	C.Xx = (C.Xx / (12.0f*sum_area)) - (size*centroid.x*centroid.x);
	C.Xy = (C.Xy / (12.0f*sum_area)) - (size*centroid.x*centroid.y);
	C.Xz = (C.Xz / (12.0f*sum_area)) - (size*centroid.x*centroid.z);
	C.Yx = (C.Yx / (12.0f*sum_area)) - (size*centroid.y*centroid.x);
	C.Yy = (C.Yy / (12.0f*sum_area)) - (size*centroid.y*centroid.y);
	C.Yz = (C.Yz / (12.0f*sum_area)) - (size*centroid.y*centroid.z);
	C.Zx = (C.Zx / (12.0f*sum_area)) - (size*centroid.z*centroid.x);
	C.Zy = (C.Zy / (12.0f*sum_area)) - (size*centroid.z*centroid.y);
	C.Zz = (C.Zz / (12.0f*sum_area)) - (size*centroid.z*centroid.z);
	
	if ( !(C.Xx || C.Xy || C.Xz) )
    {
        Vector3<TYPENAME> xAxis( Vector::crossProduct( C.Yx, C.Yy, C.Yz, C.Zx, C.Zy, C.Zz ) );
        
        C.Xx = xAxis.x;
        C.Xy = xAxis.y;
        C.Xz = xAxis.z;
    }
    else if ( !(C.Yx || C.Yy || C.Yz) )
    {
        Vector3<TYPENAME> yAxis( Vector::crossProduct( C.Zx, C.Zy, C.Zz, C.Xx, C.Xy, C.Xz ) );
        
        C.Yx = yAxis.x;
        C.Yy = yAxis.y;
        C.Yz = yAxis.z;
    }
    else if ( !(C.Zx || C.Zy || C.Zz) )
    {
        Vector3<TYPENAME> zAxis( Vector::crossProduct( C.Xx, C.Xy, C.Xz, C.Yx, C.Yy, C.Yz ) );
        
        C.Zx = zAxis.x;
        C.Zy = zAxis.y;
        C.Zz = zAxis.z;
    }  
    Vector::normalize( C.Xx, C.Xy, C.Xz );
    Vector::normalize( C.Yx, C.Yy, C.Yz );
    Vector::normalize( C.Zx, C.Zy, C.Zz );
    
	return C;
 }
 
 /**
  * getSortedRawAxes() returns a list of unit vectors denoting the OBB's raw_orientation axes of 
  * this OBBTree sorted by the axis with the longest length to the axis with the shortest 
  * length of the OBB.
  *
  * @return std::vector< Vector3<TYPENAME> >
  */
 template <typename TYPENAME>
 inline std::vector< Vector3<TYPENAME> > OBBTree<TYPENAME>::getSortedRawAxes() //bool(*sort_func)(void) )
 {
    std::vector< Vector3<TYPENAME> > axes;
    
    const TYPENAME& half_width( obb.getHalfWidth() );
    const TYPENAME& half_height( obb.getHalfHeight() );
    const TYPENAME& half_depth( obb.getHalfDepth() );
    const OrientationMatrix3<TYPENAME>& orientation( obb.getRawOrientation() );  // raw_orientation
 
    if ( half_width > half_height )
    {
        if ( half_width > half_depth )
        {
            if ( half_height > half_depth ) // half_width > half_height > half_depth
            {
                axes.push_back( orientation.getXAxis() );
                axes.push_back( orientation.getYAxis() );
                axes.push_back( orientation.getZAxis() );
            }
            else    // half_width > half_depth >= half_height
            {
                axes.push_back( orientation.getXAxis() );
                axes.push_back( orientation.getZAxis() );
                axes.push_back( orientation.getYAxis() );
            }
        }
        else // half_depth >= half_width > half_height
        {
            axes.push_back( orientation.getZAxis() );
            axes.push_back( orientation.getXAxis() );
            axes.push_back( orientation.getYAxis() );
        }
    }
    else // half_height >= half_width
    {
        if ( half_height > half_depth ) 
        {
            if ( half_width > half_depth ) // half_height >= half_width > half_depth
            {
                axes.push_back( orientation.getYAxis() );
                axes.push_back( orientation.getXAxis() );
                axes.push_back( orientation.getZAxis() );
            }
            else // half_height > half_depth >= half_width
            {
                axes.push_back( orientation.getYAxis() );
                axes.push_back( orientation.getZAxis() );
                axes.push_back( orientation.getXAxis() );
            }
        }
        else // half_depth > half_height >= half_width
        {
            axes.push_back( orientation.getZAxis() );
            axes.push_back( orientation.getYAxis() );
            axes.push_back( orientation.getXAxis() );
        }
    }
    
    return axes;
 }
 
 /**
  * splitNextLevel() splits the OBB of this OBBTree to create two children OBBTrees for this OBBTree.
  *
  * @param (const GL_Triangle<TYPENAME>* const * const) raw_triangle_Ptrs_Ptr
  * @param (const size_t) size - number of triangles
  * @param (const int) maxDepth - maximum depth of the OBBTree
  */
 template <typename TYPENAME>
 inline void OBBTree<TYPENAME>::splitNextLevel( const GL_Triangle<TYPENAME>* const * const raw_triangle_Ptrs_Ptr, 
                                                const size_t size, const int maxDepth )
 {
    //std::vector< std::pair< TYPENAME, Vector3<TYPENAME>> > axes;
    // sorted axes of the raw orientation
    // Axes with longer dimensions are at the front of the list
    std::vector< Vector3<TYPENAME> > axes( getSortedRawAxes() );

    Split_OBB_Info<TYPENAME> soi( getSplitOBBInfo( raw_triangle_Ptrs_Ptr, &triangle_Ptrs[0], size, axes[0] ) );
    
    for ( size_t i = 0x1; i < axes.size(); ++i )
    {
        // see if list was possible to split
        if ( soi.split_index > 0  && soi.split_index < size - 1 )
            break;
            
        soi = getSplitOBBInfo( raw_triangle_Ptrs_Ptr, &triangle_Ptrs[0], size, axes[i] );
    }

    /*
    Split_OBB_Info<TYPENAME> soi( getSplitOBBInfo( raw_triangles_Addresses, triangle_Ptrs, axes[0] ) );
    Split_OBB_Info<TYPENAME> soi1( getSplitOBBInfo( raw_triangles_Addresses, triangle_Ptrs, axes[1] ) );
    Split_OBB_Info<TYPENAME> soi2( getSplitOBBInfo( raw_triangles_Addresses, triangle_Ptrs, axes[2] ) );
    */
    
    /*if ( soi1 > soi )
    {
        if ( soi2 > soi1 )
        {
            soi = soi2;
        }
        else
        {
            soi = soi1;
        }
    }
    else if ( soi2 > soi )
    {
        soi = soi2;
    }*/
    
    if ( soi.split_index == 0 )
        soi.split_index = size >> 0x1; //ceil( ((GLfloat)raw_triangles.size()) / 2.0f );
    
    // L1 = < item1, item2, ...., item[index - 1] >
    // L2 = < item[index], item[index + 1], ... >
    std::vector<const GL_Triangle<TYPENAME>*> L1_raw( &soi.raw_triangle_Ptrs[ 0 ], &soi.raw_triangle_Ptrs[ soi.split_index ] );
    std::vector<const GL_Triangle<TYPENAME>*> L1( &soi.triangle_Ptrs[ 0 ], &soi.triangle_Ptrs[ soi.split_index ] );
    std::vector<const GL_Triangle<TYPENAME>*> L2_raw( &soi.raw_triangle_Ptrs[ soi.split_index ], &soi.raw_triangle_Ptrs[ size - 1 ] + 1 );
    std::vector<const GL_Triangle<TYPENAME>*> L2( &soi.triangle_Ptrs[ soi.split_index ], &soi.triangle_Ptrs[ size - 1 ] + 1 );
    
    //if ( L1.size() + L2.size() != L1_raw.size() + L2_raw.size() || L1.size() + L2.size() != raw_triangles.size() )
        //printf("L1.size(): %1d L2.size(): %2d L1_raw.size(): %3d L2_raw.size(): %4d raw_triangles.size(): %5d\n", L1.size(), L2.size(), L1_raw.size(), L2_raw.size(), raw_triangles.size() );
    
    // Spawn OBBTrees
    //leftChild() = new OBBTree<TYPENAME>( object, maxDepth, L1_raw, L1 );
    //rightChild() = new OBBTree<TYPENAME>( object, maxDepth, L2_raw, L2 );
    leftChild() = new OBBTree<TYPENAME>( object, maxDepth, &L1_raw[0], &L1[0], L1.size() );
    rightChild() = new OBBTree<TYPENAME>( object, maxDepth, &L2_raw[0], &L2[0], L2.size() );
    
 /*
    if ( height > width )
    {
        if ( depth > height )   // depth, height, width
        {
            axes.push_back( raw_orientation.getXAxis() );
            axes.push_back( raw_orientation.getYAxis() );
            axes.push_back( raw_orientation.getZAxis() );
        }
        else if ( depth > width )   // height, depth, width
        {
        
        }
        else    // height, width, depth
        {
        
        }
    }
    else if ( depth > width )   // depth, width, height
    {
        
    }
    else
    {
        if ( depth > height )   // width, depth, height
        {
        
        }
        else    // width, height, depth
        {
        
        }
    }
    */
 }
 
 /**
  * Split_OBB_Info() returns the split information for the set of raw triangles and triangles on the specified axis
  * for their specified pointers and size.
  *
  * @param (const GL_Triangle<TYPENAME>* const * const) raw_triangle_Ptrs_Ptr
  * @param (const GL_Triangle<TYPENAME>* const * const) triangle_Ptrs_Ptr
  * @param (const size_t) size - number of triangles
  * @param (const Vector3<TYPENAME>&) axis
  * @return Split_OBB_Info<TYPENAME>
  */
 template <typename TYPENAME>
 inline typename OBBTree<TYPENAME>::Split_OBB_Info<TYPENAME> OBBTree<TYPENAME>::getSplitOBBInfo( 
                                                    const GL_Triangle<TYPENAME>* const * const raw_triangle_Ptrs_Ptr, 
                                                    const GL_Triangle<TYPENAME>* const * const triangle_Ptrs_Ptr, 
                                                    const size_t size, 
                                                    const Vector3<TYPENAME>& axis ) const
 {
    std::vector< Triangle_Projection_Info<TYPENAME> > minMaxProjections;
    
    for ( size_t i( 0x0 ); i < size; ++i )
    {
        const GL_Triangle<TYPENAME>* raw_triangle_Ptr( raw_triangle_Ptrs_Ptr[i] );
        
        // find the minimum and maximum projection of the raw_triangle onto the axis of the OBB
        TYPENAME minProjection = raw_triangle_Ptr->getEndPointA() * axis;
        TYPENAME maxProjection = minProjection;
        findMinMaxProjectionExtent( raw_triangle_Ptr->getEndPointB(), axis, minProjection, maxProjection );
        findMinMaxProjectionExtent( raw_triangle_Ptr->getEndPointC(), axis, minProjection, maxProjection );
        
        Triangle_Projection_Info<TYPENAME> projectionInfo( raw_triangle_Ptr, triangle_Ptrs_Ptr[i], minProjection, maxProjection );
        
        minMaxProjections.push_back( projectionInfo );
    }
    
    // sort minMaxProjections by min, then by max
    introsort( &minMaxProjections[0], minMaxProjections.size() );
    
    // Sorting test
    /*for ( size_t i = 0x0; i < minMaxProjections.size(); ++i )
    {
        printf("sort_quad: min %1f max %2f triangle %3d\n", minMaxProjections[i].first, minMaxProjections[i].second, minMaxProjections[i].third);
        printf("size: %1d\n", minMaxProjections.size());
    }*/
    
    // Find the splitting index based on the minimal volume overlap and/or the volume summation between the 
    // two oriented-bounding boxes created by the two sets of polygons would possibly be very good, but doing 
    // so is rather computationally expensive.
    
    // Find the index with the greatest separation between maxProjection of the first triangle and
    // the minProjection of its neighboring triangle. Use median as default if all weight values tie.
    
    // find splitting point (ideally, we want the median one to minimize the depth of the OBBTree)
    // we use ceil() here to counteract the weight tie-breaker using "<" below, instead of "<="
    //size_t median( ceil( ((GLfloat)numberOfTriangles) / 2.0f ) );
    size_t index( size >> 0x1 ); // ceil( ((GLfloat)numberOfTriangles) / 2.0f ) ); //median );
    //TYPENAME maxWeight( 0.0f );
    
    // Attempt to find splitting index near the median polygon index
    
    // -->
    // Find the index of the triangle that encapsulates the triangles to
    // its left back to the median index
    while ( index < size - 1 )
    {
        TYPENAME current_Max( minMaxProjections[ index ].maxProjection );
        TYPENAME neighbor_Max( minMaxProjections[ index + 1 ].maxProjection );
        
        /*if ( current_Max == neighbor_Max )
        {
            if ( minMaxProjections[ index + 1 ].minProjection <= minMaxProjections[ index ].minProjection )
                ++index;
            else
                break;
        }
        else*/ if ( current_Max < neighbor_Max )
            ++index;
        else
            break;
    }
    
    // <--
    // Find the index of the triangle that encapsulates all triangles to its
    // right.
    while ( index > 0 )
    {
        TYPENAME current_Min( minMaxProjections[ index ].minProjection );
        TYPENAME neighbor_Min( minMaxProjections[ index - 1 ].minProjection );
        
        if ( current_Min <= neighbor_Min ) // equivalent to if ( current_Min == neighbor_Min )
            --index;
        else
            break;
    }
    
    /*
    if ( numberOfTriangles > 0x1 )
    {
        index = 0x1;
        maxWeight = minMaxProjections[ 1 ].minProjection - minMaxProjections[ 0 ].maxProjection;
    }
    
    // Attempt to find splitting index with the greatest gap between the projections onto the axis of 
    // the two sets of polygons
    for ( size_t i = 0x1; i < numberOfTriangles - 1; ++i )
    {
        TYPENAME current_Max( minMaxProjections[ i ].maxProjection );
        TYPENAME neighbor_Min( minMaxProjections[ i + 1 ].minProjection );
        
        TYPENAME currentWeight( neighbor_Min - current_Max );
        
        if ( currentWeight > maxWeight )
        {
            index = i + 1;
            maxWeight = currentWeight;
        }
        else if ( currentWeight == maxWeight )
        {
            // Weight tie-breaker
            // ideally, we want to split at the median index when the weights
            // are equal
            if ( fabs((GLfloat) i + 1 - median ) < fabs((GLfloat) index - median) )
            {
                index = i + 1;
                maxWeight = currentWeight;
            }
        }
    }*/
    
    //Split_OBB_Info<TYPENAME> soi( index, maxWeight );
    Split_OBB_Info<TYPENAME> soi( index );

    for ( size_t i( 0x0 ); i < size; ++i )
    {
        soi.raw_triangle_Ptrs.push_back( minMaxProjections[i].raw_triangle_Ptr );
        soi.triangle_Ptrs.push_back( minMaxProjections[i].triangle_Ptr );
    }
    
    //std::pair< std::vector<std::pair<const GL_Triangle<TYPENAME>*, const GL_Triangle<TYPENAME>*>>, size_t > splitInfo( sortedTriangles, index );
    
    return soi; // Split_OBB_Info<TYPENAME>
 }
 
 /** FRIEND FUNCTIONS **/
 
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 0
 /**
  * collide() returns true if the set of triangles contained within the OBBTrees intersect
  * between the OBBTrees; otherwise, false is returned.
  * If a maxDepth is specified, collide() will return true if the maximum depth is reached,
  * even though collide() has not determined whether there is a collision or not. A negative 
  * maxDepth is equivalent to having no maximum depth.
  *
  * @param (const OBBTree<TYPENAME>&) obbtreeA
  * @param (const OBBTree<TYPENAME>&) obbtreeB
  * @param (int) maxDepth
  * @return bool
  */
 template <typename TYPENAME>
 inline bool collide( const OBBTree<TYPENAME>& obbtreeA, const OBBTree<TYPENAME>& obbtreeB, int maxDepth )
 {
    if ( maxDepth > 0 )
        --maxDepth;
    else if ( maxDepth == 0 && !obbtreeA.hasChild() && !obbtreeB.hasChild() )
        return collide( obbtreeA.obb, obbtreeB.obb );
    
    if ( collide( obbtreeA.obb, obbtreeB.obb ) )
    {
        if ( obbtreeB.constLeftChild() ) // if obbtreeB has a left child
        {
            if ( obbtreeB.constRightChild() ) // if obbtreeB has a left and right child
            {
                return collide( obbtreeA, *obbtreeB.constLeftChild(), maxDepth ) 
                                            || collide( obbtreeA, *obbtreeB.constRightChild(), maxDepth );
            }
            else // if obbtreeB only has a left child
            {
                return collide( obbtreeA, *obbtreeB.constLeftChild(), maxDepth );
            }
        }
        else if ( obbtreeB.constRightChild() ) // if obbtreeB only has a right child
        {
            return collide( obbtreeA, *obbtreeB.constRightChild(), maxDepth );
        }
        else // if obbtreeB has no children
        {
            if ( obbtreeA.constLeftChild() ) // if obbtreeA has a left child
            {
                if ( obbtreeA.constRightChild() ) // if obbtreeA has a left and right child
                {
                    return collide( obbtreeB, *obbtreeA.constLeftChild(), maxDepth ) 
                                            || collide( obbtreeB, *obbtreeA.constRightChild(), maxDepth );
                }
                else // if obbtreeA only has a left child
                {
                    return collide( obbtreeB, *obbtreeA.constLeftChild(), maxDepth );
                }
            }
            else if ( obbtreeA.constRightChild() ) // if obbtreeA only has a right child
            {
                return collide( obbtreeB, *obbtreeA.constRightChild(), maxDepth );
            }
            else // neither obbtreeA nor obbtreeB has a child
            {
                // Currently, there will always be exactly one triangle in obbtreeA and obbtreeB, so there is no
                // reason for having the do-while-loop.
                
                // if any triangle from OBBTree A collides with any triangle from OBBTree B, then return true;
                // otherwise, return false.
                size_t triangle_index_A( obbtreeA.triangle_Ptrs.size() );
                if ( triangle_index_A > 0 )
                {
                    do
                    {
                        --triangle_index_A;
                        const GL_Triangle<TYPENAME>& triangleA( *obbtreeA.triangle_Ptrs[ triangle_index_A ] );
                        
                        size_t triangle_index_B( obbtreeB.triangle_Ptrs.size() );
                        if ( triangle_index_B > 0 )
                        {
                            do
                            {
                                --triangle_index_B;
                                const GL_Triangle<TYPENAME>& triangleB( *obbtreeB.triangle_Ptrs[ triangle_index_B ] );

                                if ( collide( triangleA, triangleB ) )
                                    return true;
                            } while ( triangle_index_B > 0 );
                        }
                        else
                            break;
                    } while ( triangle_index_A > 0 );
                }
            }
        }
    }
    
    return false;
 }
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 0
 
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 /**
  * collide() returns true if the set of triangles contained within the OBBTrees intersect
  * between the OBBTrees; otherwise, false is returned.
  * If a maxDepth is specified, collide() will return true if the maximum depth is reached,
  * even though collide() has not determined whether there is a collision or not. A negative 
  * maxDepth is equivalent to having no maximum depth.
  *
  * @param (OBBTree<TYPENAME>&) obbtreeA
  * @param (OBBTree<TYPENAME>&) obbtreeB
  * @param (int) maxDepth
  * @return bool
  */
 template <typename TYPENAME>
 inline bool collide( OBBTree<TYPENAME>& obbtreeA, OBBTree<TYPENAME>& obbtreeB, int maxDepth )
 {
    if ( maxDepth > 0 )
        return collide( obbtreeA, obbtreeB, maxDepth, maxDepth );
    else if ( maxDepth == 0 )
        return collide( obbtreeA.obb, obbtreeB.obb );
    else // maxDepth < 0
        return collide( obbtreeA, obbtreeB );
 }
 
 /**
  * collide() returns true if the set of triangles contained within the OBBTrees intersect
  * between the OBBTrees; otherwise, false is returned.
  *
  * @param (OBBTree<TYPENAME>&) obbtreeA
  * @param (OBBTree<TYPENAME>&) obbtreeB
  * @return bool
  */
 template <typename TYPENAME>
 inline bool collide( OBBTree<TYPENAME>& obbtreeA, OBBTree<TYPENAME>& obbtreeB )
 {
    if ( collide( obbtreeA.obb, obbtreeB.obb ) )
    {
        if ( obbtreeB.leftChild() ) // if obbtreeB has a left child
        {
            if ( obbtreeB.rightChild() ) // if obbtreeB has a left and right child
            {
                return collide( obbtreeA, *obbtreeB.leftChild() ) 
                                                || collide( obbtreeA, *obbtreeB.rightChild() );
            }
            else // if obbtreeB only has a left child
            {
                return collide( obbtreeA, *obbtreeB.leftChild() );
            }
        }
        else if ( obbtreeB.rightChild() ) // if obbtreeB only has a right child
        {
            return collide( obbtreeA, *obbtreeB.rightChild() );
        }
        else // if obbtreeB has no children
        {
            if ( obbtreeA.leftChild() ) // if obbtreeA has a left child
            {
                if ( obbtreeA.rightChild() ) // if obbtreeA has a left and right child
                {
                    return collide( obbtreeB, *obbtreeA.leftChild() ) 
                                                || collide( obbtreeB, *obbtreeA.rightChild() );
                }
                else // if obbtreeA only has a left child
                {
                    return collide( obbtreeB, *obbtreeA.leftChild() );
                }
            }
            else if ( obbtreeA.rightChild() ) // if obbtreeA only has a right child
            {
                return collide( obbtreeB, *obbtreeA.rightChild() );
            }
            else // neither obbtreeA nor obbtreeB has a child
            {
                // if any triangle from OBBTree A collides with any triangle from OBBTree B, then return true;
                // otherwise, return false.
                return collide( *obbtreeA.triangle_Ptrs[ 0 ], *obbtreeB.triangle_Ptrs[ 0 ] );
            }
        }
    }
    
    return false;
 }
 
 /**
  * collide() returns true if the set of triangles contained within the OBBTrees intersect
  * between the OBBTrees; otherwise, false is returned.
  * If a maxDepth is specified, collide() will determine if the two OBBTrees collide based
  * on the OBBs on the depth level when the maximum depth is reached. Each maxDepth is 
  * assumed to be greater than or equal to zero.
  *
  * @param (OBBTree<TYPENAME>&) obbtreeA
  * @param (OBBTree<TYPENAME>&) obbtreeB
  * @param (size_t) maxDepthA
  * @param (size_t) maxDepthB
  * @return bool
  */
 template <typename TYPENAME>
 inline bool collide( OBBTree<TYPENAME>& obbtreeA, OBBTree<TYPENAME>& obbtreeB, size_t maxDepthA, size_t maxDepthB )
 {
    if ( maxDepthA == 0 || !obbtreeA.hasChild()  )
    {
        if ( maxDepthB == 0 || !obbtreeB.hasChild() )
        {
            bool test( collide( obbtreeA.obb, obbtreeB.obb ) );
            return test;
        }
        else
        {
            return collide( obbtreeB, obbtreeA, maxDepthB, maxDepthA );
        }
    }
    
    if ( collide( obbtreeA.obb, obbtreeB.obb ) )
    {
        if ( obbtreeB.leftChild() ) // if obbtreeB has a left child
        {
            if ( obbtreeB.rightChild() ) // if obbtreeB has a left and right child
            {
                return collide( obbtreeA, *obbtreeB.leftChild(), maxDepthA, --maxDepthB ) 
                                                || collide( obbtreeA, *obbtreeB.rightChild(), maxDepthA, maxDepthB );
            }
            else // if obbtreeB only has a left child
            {
                return collide( obbtreeA, *obbtreeB.leftChild(), maxDepthA, --maxDepthB );
            }
        }
        else if ( obbtreeB.rightChild() ) // if obbtreeB only has a right child
        {
            return collide( obbtreeA, *obbtreeB.rightChild(), maxDepthA, --maxDepthB );
        }
        else // if obbtreeB has no children
        {
            if ( obbtreeA.leftChild() ) // if obbtreeA has a left child
            {
                if ( obbtreeA.rightChild() ) // if obbtreeA has a left and right child
                {
                    return collide( obbtreeB, *obbtreeA.leftChild(), maxDepthB, --maxDepthA ) 
                                                || collide( obbtreeB, *obbtreeA.rightChild(), maxDepthB, maxDepthA );
                }
                else // if obbtreeA only has a left child
                {
                    return collide( obbtreeB, *obbtreeA.leftChild(), maxDepthB, --maxDepthA );
                }
            }
            else if ( obbtreeA.rightChild() ) // if obbtreeA only has a right child
            {
                return collide( obbtreeB, *obbtreeA.rightChild(), maxDepthB, --maxDepthA );
            }
            else // neither obbtreeA nor obbtreeB has a child
            {
                printf("Error: This should never be reached (OBBTree.h)\n");
                // Currently, there will always be exactly one triangle in obbtreeA and obbtreeB, so there is no
                // reason for having the do-while-loop.
                
                // if any triangle from OBBTree A collides with any triangle from OBBTree B, then return true;
                // otherwise, return false.
                size_t triangle_index_A( obbtreeA.triangle_Ptrs.size() );
                if ( triangle_index_A > 0 )
                {
                    do
                    {
                        --triangle_index_A;
                        const GL_Triangle<TYPENAME>& triangleA( *obbtreeA.triangle_Ptrs[ triangle_index_A ] );
                        
                        size_t triangle_index_B( obbtreeB.triangle_Ptrs.size() );
                        if ( triangle_index_B > 0 )
                        {
                            do
                            {
                                --triangle_index_B;
                                const GL_Triangle<TYPENAME>& triangleB( *obbtreeB.triangle_Ptrs[ triangle_index_B ] );

                                if ( collide( triangleA, triangleB ) )
                                    return true;
                            } while ( triangle_index_B > 0 );
                        }
                        else
                            break;
                    } while ( triangle_index_A > 0 );
                }
            }
        }
    }
    
    return false;
 }
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 
 #if IMPLEMENT_INTERPOLATION == 1
 /**
  * computeCollisionInfo() returns a CollisionInfo between the two specified obbtrees.
  * If the two OBBTrees are colliding, but the difference between the specified 
  * displacementA and displacementB is the zero vector (i.e. displacementA == displacementB),
  * then the displacementA and displacementB in the returned CollisionInfo will be set to the 
  * normal vectors of the location of intersection on OBBTreeB and OBBTreeA, but in the 
  * reverse directions, respectively. 
  * A negative maxDepth is equivalent to having no maximum depth.
  *
  * @param (OBBTree<TYPENAME>&) obbtreeA - OBBTree of object A
  * @param (OBBTree<TYPENAME>&) obbtreeB - OBBTree of object B
  * @param (int) maxDepth
  * @param (Vector3<TYPENAME>) displacementA - the current position of object A minus the previous position of object A
  * @param (Vector3<TYPENAME>) displacementB - the current position of object B minus the previous position of object B
  * @return CollisionInfo<TYPENAME>
  */
 template <typename TYPENAME>
 inline CollisionInfo<TYPENAME> computeCollisionInfo( OBBTree<TYPENAME>& obbtreeA, OBBTree<TYPENAME>& obbtreeB, int maxDepth, 
                                                      Vector3<TYPENAME> displacementA, Vector3<TYPENAME> displacementB )
 {
    if ( maxDepth > 0 )
        return computeCollisionInfo( obbtreeA, obbtreeB, maxDepth, maxDepth, displacementA, displacementB );
    else if ( maxDepth == 0 )
    {
        TYPENAME extent( collisionExtent( obbtreeA.obb, obbtreeB.obb, displacementA, displacementB ) );
        return CollisionInfo<TYPENAME>( extent, displacementA, displacementB, obbtreeA.obb, obbtreeB.obb, GL_Triangle<TYPENAME>(0,0,0,0,0,0,0,0,0), GL_Triangle<TYPENAME>(0,0,0,0,0,0,0,0,0) );
    }
    else // maxDepth < 0
        return computeCollisionInfo( obbtreeA, obbtreeB, displacementA, displacementB );
 }
 
 /**
  * computeCollisionInfo() returns the CollisionInfo between the two specified OBBs, 
  * including the extent that the object need to move in the directions toward their 
  * previous positions to avoid the collision; if the extent returned is less than zero, 
  * then the two object are currently not intersecting. If the two OBBTrees are 
  * colliding, but the difference between the specified displacementA and displacementB is 
  * the zero vector (i.e. displacementA == displacementB), then the displacementA and 
  * displacementB will be modified to the normal vectors of the location of intersection on 
  * OBBTreeB and OBBTreeA, but in the reverse directions, respectively.
  *
  * @param (OBBTree<TYPENAME>&) obbtreeA - OBBTree of object A
  * @param (OBBTree<TYPENAME>&) obbtreeB - OBBTree of object B
  * @param (Vector3<TYPENAME>) displacementA - the current position of object A minus the previous position of object A
  * @param (Vector3<TYPENAME>) displacementB - the current position of object B minus the previous position of object B
  * @return CollisionInfo<TYPENAME>
  */
 template <typename TYPENAME>
 inline CollisionInfo<TYPENAME> computeCollisionInfo( OBBTree<TYPENAME>& obbtreeA, OBBTree<TYPENAME>& obbtreeB, 
                                                      Vector3<TYPENAME> displacementA, Vector3<TYPENAME> displacementB )
 {
    if ( collide( obbtreeA.obb, obbtreeB.obb ) )
    {
        if ( obbtreeB.leftChild() ) // if obbtreeB has a left child
        {
            if ( obbtreeB.rightChild() ) // if obbtreeB has a left and right child
            {
                CollisionInfo<TYPENAME> c_info1( computeCollisionInfo( obbtreeA, *obbtreeB.leftChild(), displacementA, displacementB ) );
                CollisionInfo<TYPENAME> c_info2( computeCollisionInfo( obbtreeA, *obbtreeB.rightChild(), displacementA, displacementB ) );
                return MAX( c_info1, c_info2 );
            }
            else // if obbtreeB only has a left child
            {
                return computeCollisionInfo( obbtreeA, *obbtreeB.leftChild(), displacementA, displacementB );
            }
        }
        else if ( obbtreeB.rightChild() ) // if obbtreeB only has a right child
        {
            return computeCollisionInfo( obbtreeA, *obbtreeB.rightChild(), displacementA, displacementB );
        }
        else // if obbtreeB has no children
        {
            if ( obbtreeA.leftChild() ) // if obbtreeA has a left child
            {
                if ( obbtreeA.rightChild() ) // if obbtreeA has a left and right child
                {
                    CollisionInfo<TYPENAME> c_info1( computeCollisionInfo( *obbtreeA.leftChild(), obbtreeB, displacementA, displacementB ) );
                    CollisionInfo<TYPENAME> c_info2( computeCollisionInfo( *obbtreeA.rightChild(), obbtreeB, displacementA, displacementB ) );
                    return MAX( c_info1, c_info2 );
                }
                else // if obbtreeA only has a left child
                {
                    return computeCollisionInfo( *obbtreeA.leftChild(), obbtreeB, displacementA, displacementB );
                }
            }
            else if ( obbtreeA.rightChild() ) // if obbtreeA only has a right child
            {
                return computeCollisionInfo( *obbtreeA.rightChild(), obbtreeB, displacementA, displacementB );
            }
            else // neither obbtreeA nor obbtreeB has a child
            { 
                // Currently, there will always be exactly one triangle in obbtreeA and obbtreeB, so there is no
                // reason for having the do-while-loop.
                
                // if any triangle from OBBTree A collides with any triangle from OBBTree B, then return true;
                // otherwise, return false.
// WARNING: TEST
                //printf("Test OBBTree.h\n");
                //if ( obbtreeA.triangle_Ptrs.size() > 1 || obbtreeB.triangle_Ptrs.size() > 1 )
                    //printf( "Error: OBBTree.h - size of triangle_Ptrs > 1, when should be exactly 1\n" );
                
                TYPENAME extent( collisionExtent( *obbtreeA.triangle_Ptrs[ 0 ], *obbtreeB.triangle_Ptrs[ 0 ], 
                                                                                displacementA, displacementB ) );
                return CollisionInfo<TYPENAME>( extent, displacementA, displacementB, obbtreeA.obb, obbtreeB.obb, *obbtreeA.triangle_Ptrs[ 0 ], *obbtreeB.triangle_Ptrs[ 0 ] );
            }
        }
    }
    
    return CollisionInfo<TYPENAME>( -1.0f, displacementA, displacementB, obbtreeA.obb, obbtreeB.obb, GL_Triangle<TYPENAME>(0,0,0,0,0,0,0,0,0), GL_Triangle<TYPENAME>(0,0,0,0,0,0,0,0,0) );
 }
 
 /**
  * computeCollisionInfo() returns the CollisionInfo between the two specified OBBs, 
  * including the extent that the object need to move in the directions toward their 
  * previous positions to avoid the collision; if the extent returned is less than zero, 
  * then the two object are currently not intersecting. Zero is returned If the two 
  * OBBTrees are colliding, but the difference between the specified displacementA and 
  * displacementB is the zero vector (i.e. displacementA == displacementB), then the 
  * displacementA and displacementB will be modified to the normal vectors of the location 
  * of intersection on OBBTreeB and OBBTreeA, but in the reverse directions, respectively.
  * If a maxDepth is specified, collisionExtent() will return the extent when the maximum 
  * depth is reached for each specified OBBTree. Each maxDepth must be greater than or equal 
  * to zero.
  *
  * @param (OBBTree<TYPENAME>&) obbtreeA - OBBTree of object A
  * @param (OBBTree<TYPENAME>&) obbtreeB - OBBTree of object B
  * @param (size_t) maxDepthA
  * @param (size_t) maxDepthB
  * @param (Vector3<TYPENAME>) displacementA - the current position of object A minus the previous position of object A
  * @param (Vector3<TYPENAME>) displacementB - the current position of object B minus the previous position of object B
  * @return CollisionInfo<TYPENAME>
  */
 template <typename TYPENAME>
 inline CollisionInfo<TYPENAME> computeCollisionInfo( OBBTree<TYPENAME>& obbtreeA, OBBTree<TYPENAME>& obbtreeB, 
                      size_t maxDepthA, size_t maxDepthB, Vector3<TYPENAME> displacementA, Vector3<TYPENAME> displacementB )
 {
    // Include upper-level leaf nodes as possible colliding OBBs
    if ( maxDepthA == 0 || !obbtreeA.hasChild()  )
    {
        if ( maxDepthB == 0 || !obbtreeB.hasChild() )
        {
            TYPENAME extent( collisionExtent( obbtreeA.obb, obbtreeB.obb, displacementA, displacementB ) );
            return CollisionInfo<TYPENAME>( extent, displacementA, displacementB, obbtreeA.obb, obbtreeB.obb, GL_Triangle<TYPENAME>(0,0,0,0,0,0,0,0,0), GL_Triangle<TYPENAME>(0,0,0,0,0,0,0,0,0) );
        }
    }
    
    /*// Exclude upper-level leaf nodes as possible colliding OBBs
    if ( maxDepthA == 0 ) // || !obbtreeA.hasChild()  )
    {
        if ( maxDepthB == 0 ) // || !obbtreeB.hasChild() )
        {
            TYPENAME extent( collisionExtent( obbtreeA.obb, obbtreeB.obb, displacementA, displacementB ) );
            return CollisionInfo<TYPENAME>( extent, displacementA, displacementB, obbtreeA.obb, obbtreeB.obb, GL_Triangle<TYPENAME>(0,0,0,0,0,0,0,0,0), GL_Triangle<TYPENAME>(0,0,0,0,0,0,0,0,0) );
        }
        else if ( !obbtreeB.hasChild() )
        {
            return CollisionInfo<TYPENAME>( -1, displacementA, displacementB, obbtreeA.obb, obbtreeB.obb, GL_Triangle<TYPENAME>(0,0,0,0,0,0,0,0,0), GL_Triangle<TYPENAME>(0,0,0,0,0,0,0,0,0) );
        }
    }
    else if ( !obbtreeA.hasChild() )
    {
        return CollisionInfo<TYPENAME>( -1, displacementA, displacementB, obbtreeA.obb, obbtreeB.obb, GL_Triangle<TYPENAME>(0,0,0,0,0,0,0,0,0), GL_Triangle<TYPENAME>(0,0,0,0,0,0,0,0,0) );
    }*/
    
    if ( collide( obbtreeA.obb, obbtreeB.obb ) )
    {
        if ( obbtreeB.leftChild() ) // if obbtreeB has a left child
        {
            if ( obbtreeB.rightChild() ) // if obbtreeB has a left and right child
            {
                CollisionInfo<TYPENAME> c1 ( computeCollisionInfo( obbtreeA, *obbtreeB.leftChild(), maxDepthA, --maxDepthB, displacementA, displacementB ) );
                CollisionInfo<TYPENAME> c2 ( computeCollisionInfo( obbtreeA, *obbtreeB.rightChild(), maxDepthA, maxDepthB, displacementA, displacementB ) );
                return MAX( c1, c2 );
            }
            else // if obbtreeB only has a left child
            {
                return computeCollisionInfo( obbtreeA, *obbtreeB.leftChild(), maxDepthA, --maxDepthB, displacementA, displacementB );
            }
        }
        else if ( obbtreeB.rightChild() ) // if obbtreeB only has a right child
        {
            return computeCollisionInfo( obbtreeA, *obbtreeB.rightChild(), maxDepthA, --maxDepthB, displacementA, displacementB );
        }
        else // if obbtreeB has no children
        {
            if ( obbtreeA.leftChild() ) // if obbtreeA has a left child
            {
                if ( obbtreeA.rightChild() ) // if obbtreeA has a left and right child
                {
                    CollisionInfo<TYPENAME> c1 ( computeCollisionInfo( *obbtreeA.leftChild(), obbtreeB, --maxDepthA, maxDepthB, displacementA, displacementB ) );
                    CollisionInfo<TYPENAME> c2 ( computeCollisionInfo( *obbtreeA.rightChild(), obbtreeB, maxDepthA, maxDepthB, displacementA, displacementB ) );
                    return MAX( c1, c2 );
                }
                else // if obbtreeA only has a left child
                {
                    return computeCollisionInfo( *obbtreeA.leftChild(), obbtreeB, --maxDepthA, maxDepthB, displacementA, displacementB );
                }
            }
            else if ( obbtreeA.rightChild() ) // if obbtreeA only has a right child
            {
                return computeCollisionInfo( *obbtreeA.rightChild(), obbtreeB, --maxDepthA, maxDepthB, displacementA, displacementB );
            }
            else // neither obbtreeA nor obbtreeB has a child
            {
                // Currently, there will always be exactly one triangle in obbtreeA and obbtreeB, so there is no
                // reason for having the do-while-loop.
                
                // if any triangle from OBBTree A collides with any triangle from OBBTree B, then return true;
                // otherwise, return false.
// WARNING: TEST
                printf("Error: This should never be reached (OBBTree.h)\n");
                //printf("Test OBBTree.h\n");
                //if ( obbtreeA.triangle_Ptrs.size() > 1 || obbtreeB.triangle_Ptrs.size() > 1 )
                    //printf( "Error: OBBTree.h - size of triangle_Ptrs > 1, when should be exactly 1\n" );
                
                TYPENAME extent( collisionExtent( *obbtreeA.triangle_Ptrs[ 0 ], *obbtreeB.triangle_Ptrs[ 0 ], 
                                                                                displacementA, displacementB ) );
                return CollisionInfo<TYPENAME>( extent, displacementA, displacementB, obbtreeA.obb, obbtreeB.obb, *obbtreeA.triangle_Ptrs[ 0 ], *obbtreeB.triangle_Ptrs[ 0 ] );
            }
        }
    }
    
    return CollisionInfo<TYPENAME>( -1, displacementA, displacementB, obbtreeA.obb, obbtreeB.obb, GL_Triangle<TYPENAME>(0,0,0,0,0,0,0,0,0), GL_Triangle<TYPENAME>(0,0,0,0,0,0,0,0,0) );
 }
 
 /**
  * collisionExtent() returns the extent that the object need to move in the directions
  * toward their previous positions to avoid the collision; if the extent returned is less
  * than zero, then the two object are currently not intersecting. Zero is returned if 
  * the two OBBTrees of the objects are colliding, but the difference between the specified 
  * displacementA and displacementB is the zero vector (i.e. displacementA == displacementB).
  * If a maxDepth is specified, collisionExtent() will return the extent when the maximum 
  * depth is reached. A negative maxDepth is equivalent to having no maximum depth.
  *
  * @param (OBBTree<TYPENAME>&) obbtreeA - OBBTree of object A
  * @param (OBBTree<TYPENAME>&) obbtreeB - OBBTree of object B
  * @param (int) maxDepth
  * @param (const Vector3<TYPENAME>&) displacementA - the current position of object A minus the previous position of object A
  * @param (const Vector3<TYPENAME>&) displacementB - the current position of object B minus the previous position of object B
  * @return TYPENAME
  */
 template <typename TYPENAME>
 inline TYPENAME collisionExtent( OBBTree<TYPENAME>& obbtreeA, OBBTree<TYPENAME>& obbtreeB, int maxDepth, 
                                            const Vector3<TYPENAME>& displacementA, const Vector3<TYPENAME>& displacementB )
 {
    if ( maxDepth > 0 )
        return collisionExtent( obbtreeA, obbtreeB, maxDepth, maxDepth, displacementA, displacementB );
    else if ( maxDepth == 0 ) //&& !obbtreeA.hasChild() && !obbtreeB.hasChild() )
        return collisionExtent( obbtreeA.obb, obbtreeB.obb, displacementA, displacementB );
    else // maxDepth < 0
        return collisionExtent( obbtreeA, obbtreeB, displacementA, displacementB );
 }
 
 /**
  * collisionExtent() returns the extent that the object need to move in the directions
  * toward their previous positions to avoid the collision; if the extent returned is less
  * than zero, then the two object are currently not intersecting. Zero is returned if 
  * the two OBBTrees of the objects are colliding, but the difference between the specified 
  * displacementA and displacementB is the zero vector (i.e. displacementA == displacementB).
  *
  * @param (OBBTree<TYPENAME>&) obbtreeA - OBBTree of object A
  * @param (OBBTree<TYPENAME>&) obbtreeB - OBBTree of object B
  * @param (const Vector3<TYPENAME>&) displacementA - the current position of object A minus the previous position of object A
  * @param (const Vector3<TYPENAME>&) displacementB - the current position of object B minus the previous position of object B
  * @return TYPENAME
  */
 template <typename TYPENAME>
 inline TYPENAME collisionExtent( OBBTree<TYPENAME>& obbtreeA, OBBTree<TYPENAME>& obbtreeB, 
                                            const Vector3<TYPENAME>& displacementA, const Vector3<TYPENAME>& displacementB )
 {
    if ( collide( obbtreeA.obb, obbtreeB.obb ) )
    {
        if ( obbtreeB.leftChild() ) // if obbtreeB has a left child
        {
            if ( obbtreeB.rightChild() ) // if obbtreeB has a left and right child
            {
                TYPENAME extent1( collisionExtent( obbtreeA, *obbtreeB.leftChild(), displacementA, displacementB ) );
                TYPENAME extent2( collisionExtent( obbtreeA, *obbtreeB.rightChild(), displacementA, displacementB ) );
                return MAX( extent1, extent2 );
            }
            else // if obbtreeB only has a left child
            {
                return collisionExtent( obbtreeA, *obbtreeB.leftChild(), displacementA, displacementB );
            }
        }
        else if ( obbtreeB.rightChild() ) // if obbtreeB only has a right child
        {
            return collisionExtent( obbtreeA, *obbtreeB.rightChild(), displacementA, displacementB );
        }
        else // if obbtreeB has no children
        {
            if ( obbtreeA.leftChild() ) // if obbtreeA has a left child
            {
                if ( obbtreeA.rightChild() ) // if obbtreeA has a left and right child
                {
                    TYPENAME extent1( collisionExtent( obbtreeB, *obbtreeA.leftChild(), displacementB, displacementA ) );
                    TYPENAME extent2( collisionExtent( obbtreeB, *obbtreeA.rightChild(), displacementB, displacementA ) );
                    return MAX( extent1, extent2 );
                }
                else // if obbtreeA only has a left child
                {
                    return collisionExtent( obbtreeB, *obbtreeA.leftChild(), displacementB, displacementA );
                }
            }
            else if ( obbtreeA.rightChild() ) // if obbtreeA only has a right child
            {
                return collisionExtent( obbtreeB, *obbtreeA.rightChild(), displacementB, displacementA );
            }
            else // neither obbtreeA nor obbtreeB has a child
            { 
                // Currently, there will always be exactly one triangle in obbtreeA and obbtreeB, so there is no
                // reason for having the do-while-loop.
                
                // if any triangle from OBBTree A collides with any triangle from OBBTree B, then return true;
                // otherwise, return false.
// WARNING: TEST
                //printf("Test OBBTree.h\n");
                //if ( obbtreeA.triangle_Ptrs.size() > 1 || obbtreeB.triangle_Ptrs.size() > 1 )
                    //printf( "Error: OBBTree.h - size of triangle_Ptrs > 1, when should be exactly 1\n" );
                
                return collisionExtent( *obbtreeA.triangle_Ptrs[ 0 ], *obbtreeB.triangle_Ptrs[ 0 ], 
                                        displacementA, displacementB );
            }
        }
    }
    
    return -1.0f;
 }
 
 /**
  * collisionExtent() returns the extent that the object need to move in the directions
  * toward their previous positions to avoid the collision; if the extent returned is less
  * than zero, then the two object are currently not intersecting. Zero is returned if 
  * the two OBBTrees of the objects are colliding, but the difference between the specified 
  * displacementA and displacementB is the zero vector (i.e. displacementA == displacementB).
  * If a maxDepth is specified, collisionExtent() will return the extent when the maximum 
  * depth is reached for each specified OBBTree. Each maxDepth must be greater than or equal 
  * to zero.
  *
  * @param (OBBTree<TYPENAME>&) obbtreeA - OBBTree of object A
  * @param (OBBTree<TYPENAME>&) obbtreeB - OBBTree of object B
  * @param (size_t) maxDepthA
  * @param (size_t) maxDepthB
  * @param (const Vector3<TYPENAME>&) displacementA - the current position of object A minus the previous position of object A
  * @param (const Vector3<TYPENAME>&) displacementB - the current position of object B minus the previous position of object B
  * @return TYPENAME
  */
 template <typename TYPENAME>
 inline TYPENAME collisionExtent( OBBTree<TYPENAME>& obbtreeA, OBBTree<TYPENAME>& obbtreeB, size_t maxDepthA, 
                            size_t maxDepthB, const Vector3<TYPENAME>& displacementA, const Vector3<TYPENAME>& displacementB )
 {
    if ( maxDepthA == 0 || !obbtreeA.hasChild()  )
    {
        if ( maxDepthB == 0 || !obbtreeB.hasChild() )
            return collisionExtent( obbtreeA.obb, obbtreeB.obb, displacementA, displacementB );
        else
            return collisionExtent( obbtreeB, obbtreeA, maxDepthB, maxDepthA, displacementB, displacementA );
    }
    
    if ( collide( obbtreeA.obb, obbtreeB.obb ) )
    {
        if ( obbtreeB.leftChild() ) // if obbtreeB has a left child
        {
            if ( obbtreeB.rightChild() ) // if obbtreeB has a left and right child
            {
                TYPENAME extent1( collisionExtent( obbtreeA, *obbtreeB.leftChild(), maxDepthA, --maxDepthB, displacementA, displacementB ) );
                TYPENAME extent2( collisionExtent( obbtreeA, *obbtreeB.rightChild(), maxDepthA, maxDepthB, displacementA, displacementB ) );
                return MAX( extent1, extent2 );
            }
            else // if obbtreeB only has a left child
            {
                return collisionExtent( obbtreeA, *obbtreeB.leftChild(), maxDepthA, --maxDepthB, displacementA, displacementB );
            }
        }
        else if ( obbtreeB.rightChild() ) // if obbtreeB only has a right child
        {
            return collisionExtent( obbtreeA, *obbtreeB.rightChild(), maxDepthA, --maxDepthB, displacementA, displacementB );
        }
        else // if obbtreeB has no children
        {
            if ( obbtreeA.leftChild() ) // if obbtreeA has a left child
            {
                if ( obbtreeA.rightChild() ) // if obbtreeA has a left and right child
                {
                    TYPENAME extent1( collisionExtent( obbtreeB, *obbtreeA.leftChild(), maxDepthB, --maxDepthA, displacementB, displacementA ) );
                    TYPENAME extent2( collisionExtent( obbtreeB, *obbtreeA.rightChild(), maxDepthB, maxDepthA, displacementB, displacementA ) );
                    return MAX( extent1, extent2 );
                }
                else // if obbtreeA only has a left child
                {
                    return collisionExtent( obbtreeB, *obbtreeA.leftChild(), maxDepthB, --maxDepthA, displacementB, displacementA );
                }
            }
            else if ( obbtreeA.rightChild() ) // if obbtreeA only has a right child
            {
                return collisionExtent( obbtreeB, *obbtreeA.rightChild(), maxDepthB, --maxDepthA, displacementB, displacementA );
            }
            else // neither obbtreeA nor obbtreeB has a child
            { 
                // Currently, there will always be exactly one triangle in obbtreeA and obbtreeB, so there is no
                // reason for having the do-while-loop.
                
                // if any triangle from OBBTree A collides with any triangle from OBBTree B, then return true;
                // otherwise, return false.
// WARNING: TEST
                printf("Error: This should never be reached (OBBTree.h)\n");
                //printf("Test OBBTree.h\n");
                //if ( obbtreeA.triangle_Ptrs.size() > 1 || obbtreeB.triangle_Ptrs.size() > 1 )
                    //printf( "Error: OBBTree.h - size of triangle_Ptrs > 1, when should be exactly 1\n" );
                
                return collisionExtent( *obbtreeA.triangle_Ptrs[ 0 ], *obbtreeB.triangle_Ptrs[ 0 ], 
                                        displacementA, displacementB );
            }
        }
    }
    
    return -1.0f;
 }
 #endif // IMPLEMENT_INTERPOLATION == 1
 
 /**
  * maintain() maintains the OBB of the specified OBBTree and maintain
  * all OBBTrees that are below this OBBTree in the hierarchy.
  *
  * @param (OBBTree<TYPENAME>&) obbtree
  */
 template <typename TYPENAME>
 inline void maintain( OBBTree<TYPENAME>& obbtree )
 {
 /*#if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
    if ( !obbtree.isMaintained )    // maintain only if the OBBTree has not been maintained yet
    {
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1*/
 
        maintain( obbtree.obb );
 
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 0
        if ( obbtree.leftChild() != NULL )
            maintain( *obbtree.leftChild() );
        if ( obbtree.rightChild() != NULL )
            maintain( *obbtree.rightChild() );
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 0
 
 /*#if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
        obbtree.isMaintained = true;   // mark as maintained
    }
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1*/
 }
 
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 /**
  * markForMaintenance() marks the OBB of this OBBTree to
  * be maintained (i.e. recalculate the orientation and
  * position of the OBB).
  *
  * @param (OBBTree<TYPENAME>&) obbtree
  */
 template <typename TYPENAME>
 inline void markForMaintenance( OBBTree<TYPENAME>& obbtree )
 {
    if ( obbtree.obb.isMaintained )
    {
        obbtree.obb.isMaintained = false;
        
        if ( obbtree.constLeftChild() != NULL )
            markForMaintenance( *obbtree.leftChild() );
        if ( obbtree.constRightChild() != NULL )
            markForMaintenance( *obbtree.rightChild() );
    }
    
    // else if obbtree.obb has not been maintained yet,
    // then all its children also have not been maintained yet.
 }
 #endif GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 
 /**
  * print() prints out information about the OBBTree.
  *
  * @param (const OBBTree<TYPENAME>&) obbtree
  * @param (GLint) depth
  */
 template <typename TYPENAME> 
 inline void print( const OBBTree<TYPENAME>& obbtree, GLint depth )
 {
    printf("OBBTree Depth %1d:\n", depth);
    printf("    LeftChild: %1d\n", obbtree.constLeftChild() );
    printf("    RightChild: %1d\n", obbtree.constRightChild() );
    if ( obbtree.constLeftChild() )
        print( *obbtree.constLeftChild(), ++depth );
    if ( obbtree.constRightChild() )
        print( *obbtree.constRightChild(), depth );
 }
 
 /**
  * render() contains the code necessary to render an OBBTree in OpenGL.
  *
  * @param (const OBBTree<TYPENAME>&) obbtree
  * @param (GLint) depth
  */
 template <typename TYPENAME>
 inline void render( const OBBTree<TYPENAME>& obbtree, GLint depth )
 {
    if ( depth == 0 ) // if depth == 0, do not render any OBB
    {
        return;
    }
    else if ( depth == 1 )  // if depth == 1, render only this depth level
    {
        render( obbtree.obb );  // render the OBB
    }
    else if ( depth < 0 )    // if depth < 0, render all depth levels
    {
        render( obbtree.obb );  // render the OBB
        
        // render next depth level
        if ( obbtree.constLeftChild() != NULL )
            render( *obbtree.constLeftChild(), depth ); // render left OBBTree
        if ( obbtree.constRightChild() != NULL )
            render( *obbtree.constRightChild(), depth ); // render right OBBTree
    }
    else // otherwise, do not render this depth level; check if should render next depth level
    {
        --depth; // decrement depth
        if ( obbtree.constLeftChild() != NULL )
            render( *obbtree.constLeftChild(), depth ); // check next depth level's left OBBTree
        if ( obbtree.constRightChild() != NULL )
            render( *obbtree.constRightChild(), depth ); // check next depth level's of right OBBTree
    }
    
 }
 
 #endif // OBBTREE_H